diff --git a/iri_foot/include/foot_alg.h b/iri_foot/include/foot_alg.h
index 3b408508b29de640b603dc1c99edd2f544a12e09..7f89c38e4cf1d5ac7ad91e7e2564ae5bcab748dd 100644
--- a/iri_foot/include/foot_alg.h
+++ b/iri_foot/include/foot_alg.h
@@ -127,7 +127,7 @@ class FootAlgorithm
     *
     */
     ~FootAlgorithm(void);
-    double angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4);
+    double angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
 };
 
 #endif
diff --git a/iri_foot/include/foot_alg.h~ b/iri_foot/include/foot_alg.h~
index bc34b98336767796f035cb29c0302c843de2bbc8..3b408508b29de640b603dc1c99edd2f544a12e09 100644
--- a/iri_foot/include/foot_alg.h~
+++ b/iri_foot/include/foot_alg.h~
@@ -127,7 +127,7 @@ class FootAlgorithm
     *
     */
     ~FootAlgorithm(void);
-    double angle(tf::Vector p1, tf::Vector p2, tf::Vector p3, tf::Vector p4);
+    double angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4);
 };
 
 #endif
diff --git a/iri_foot/include/foot_alg_node.h~ b/iri_foot/include/foot_alg_node.h~
index 6584c2180e672dc668ca7d7769c94dfae460504f..a7524c8f1f41eb1456231236443192bf0c641f37 100644
--- a/iri_foot/include/foot_alg_node.h~
+++ b/iri_foot/include/foot_alg_node.h~
@@ -31,6 +31,7 @@
 
 // [publisher subscriber headers]
 #include <std_msgs/String.h>
+#include <std_msgs/Float64.h>
 
 // [service client headers]
 
@@ -54,6 +55,9 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
     tf::Vector3 rightknee;
 		
     // [publisher attributes]
+    ros::Publisher myfeet_publisher_;
+    std_msgs::String myfeet_String_msg_;
+
 
     // [subscriber attributes]
 
diff --git a/iri_foot/src/foot_alg.cpp b/iri_foot/src/foot_alg.cpp
index a2a7bbabfa15a5bfbd0c5627092c4d89abe281d1..789e83b65fbf73dd4f5588563aa312c44c5f63bd 100644
--- a/iri_foot/src/foot_alg.cpp
+++ b/iri_foot/src/foot_alg.cpp
@@ -23,15 +23,13 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
 
 // FootAlgorithm Public API
 
-double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
+void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
 {
 	double numleft = p2.getZ() - p1.getZ(); 
 	double numright = p4.getZ() - p3.getZ();
 	double denleft = p2.getY() - p1.getY();
 	double denright = p4.getY() - p3.getY();
-	double angleft = atan (numleft/denleft);
-	double angright = atan (numright/denright);
-	return angleft;
-	return angright;
+	angleft = atan (numleft/denleft);
+	angright = atan (numright/denright);
 }
 
diff --git a/iri_foot/src/foot_alg.cpp~ b/iri_foot/src/foot_alg.cpp~
index a2a7bbabfa15a5bfbd0c5627092c4d89abe281d1..789e83b65fbf73dd4f5588563aa312c44c5f63bd 100644
--- a/iri_foot/src/foot_alg.cpp~
+++ b/iri_foot/src/foot_alg.cpp~
@@ -23,15 +23,13 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
 
 // FootAlgorithm Public API
 
-double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
+void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
 {
 	double numleft = p2.getZ() - p1.getZ(); 
 	double numright = p4.getZ() - p3.getZ();
 	double denleft = p2.getY() - p1.getY();
 	double denright = p4.getY() - p3.getY();
-	double angleft = atan (numleft/denleft);
-	double angright = atan (numright/denright);
-	return angleft;
-	return angright;
+	angleft = atan (numleft/denleft);
+	angright = atan (numright/denright);
 }
 
diff --git a/iri_foot/src/foot_alg_node.cpp b/iri_foot/src/foot_alg_node.cpp
index 6f916cdc43c8802cc1543346055d1d006d274cc1..6dd43122a75d3806a58dd3be7b72961b9c4e5010 100644
--- a/iri_foot/src/foot_alg_node.cpp
+++ b/iri_foot/src/foot_alg_node.cpp
@@ -1,4 +1,5 @@
 #include "foot_alg_node.h"
+#include "foot_alg.cpp"
 using namespace std;
 #include <string>
 
@@ -64,7 +65,12 @@ try{
 	listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
 	rightknee = trasnform4.getOrigin();
 	
-
+	
+	//Apply the function that measures the angle between the knee and the foot. When pass the limit (angle equals 30 degrees) the publisher says us that the foot is extended. 
+	double angleft = 0.0;
+	double angright = 0.0;
+	angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
+	if (angleft>30.0)
 
 
 
diff --git a/iri_foot/src/foot_alg_node.cpp~ b/iri_foot/src/foot_alg_node.cpp~
index 6f916cdc43c8802cc1543346055d1d006d274cc1..9ffde8ddb1cc432cfb73d78cc6ee4d8f343a7458 100644
--- a/iri_foot/src/foot_alg_node.cpp~
+++ b/iri_foot/src/foot_alg_node.cpp~
@@ -1,4 +1,5 @@
 #include "foot_alg_node.h"
+#include "foot_alg.cpp"
 using namespace std;
 #include <string>
 
@@ -64,7 +65,12 @@ try{
 	listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
 	rightknee = trasnform4.getOrigin();
 	
-
+	
+	//Apply the function that measures the angle between the knee and the foot. When pass the limit (angle equals 30 degrees) the publisher says us that the foot is extended. 
+	double angleft = 0.0;
+	double angright = 0.0;
+	angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
+