I have added the publisher and I'm doing the function

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

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

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]
 

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);
 }
 

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);
 }
 

iri_foot/src/foot_alg_node.cpp

 #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)
 
 
 

iri_foot/src/foot_alg_node.cpp~

 #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);
+