I add the publisher and I'm doing the function

hola.cpp

-int main ()
-{
-}

iri_foot/CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs tf)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -93,7 +93,8 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 # ******************************************************************** 
 #               Add message headers dependencies 
 # ******************************************************************** 
-# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+#add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
 # ******************************************************************** 

iri_foot/CMakeLists.txt~

+cmake_minimum_required(VERSION 2.8.3)
+project(iri_foot)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs tf)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+# find_package(<dependency> REQUIRED)
+
+# ******************************************************************** 
+#           Add topic, service and action definition here
+# ******************************************************************** 
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+# ******************************************************************** 
+#                 Add the dynamic reconfigure file 
+# ******************************************************************** 
+generate_dynamic_reconfigure_options(cfg/Foot.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+ CATKIN_DEPENDS iri_base_algorithm
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+#  DEPENDS 
+)
+
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/foot_alg.cpp src/foot_alg_node.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ******************************************************************** 
+#               Add message headers dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})

iri_foot/include/foot_alg.h

@@ -26,6 +26,7 @@
 #define _foot_alg_h_
 
 #include <iri_foot/FootConfig.h>
+#include <tf/transform_listener.h>
 
 //include foot_alg main library
 
@@ -126,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.h~

@@ -26,6 +26,7 @@
 #define _foot_alg_h_
 
 #include <iri_foot/FootConfig.h>
+#include <tf/transform_listener.h>
 
 //include foot_alg main library
 
@@ -126,7 +127,7 @@ class FootAlgorithm
     *
     */
     ~FootAlgorithm(void);
-    double angle(tf::Vector );
+    double angle(tf::Vector p1, tf::Vector p2, tf::Vector p3, tf::Vector 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/include/foot_alg_node.h~

@@ -44,7 +44,10 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
 {
   private:
     tf::TransformListener listener;
-
+    tf::StampedTransform transform1;
+    tf::StampedTransform transform2;
+    tf::StampedTransform transform3;
+    tf::StampedTransform transform4;
     tf::Vector3 rightfoot;
     tf::Vector3 leftfoot;
     tf::Vector3 leftknee;

iri_foot/src/foot_alg.cpp

@@ -25,14 +25,13 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
 
 double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
 {
-	const double PI = 3.14159265359.
-	double numleft = (p1.getX() * p2.getX()) + (p1.getY() * p2.getY()) + (p1.getZ() * p2.getZ());
-	double numright = (p3.getX() * p4.getX()) + (p3.getY() * p4.getY()) + (p3.getZ() * p4.getZ());
-	double denleft = sqrt((p1.getX() * p1.getX()) + (p1.getY() * p1.getY()) + (p1.getZ() * p1.getZ())) * sqrt((p2.getX() * p2.getX()) + (p2.getY() * p2.getY()) + (p2.getZ() * p2.getZ()));
-	double denright = sqrt((p3.getX() * p3.getX()) + (p3.getY() * p3.getY()) + (p3.getZ() * p3.getZ())) * sqrt((p4.getX() * p4.getX()) + (p4.getY() * p4.getY()) + (p4.getZ() * p4.getZ()));
-	double angleft = acos (numleft/denleft);
-	double angright = acos (numright/denright);
-	
-	
+	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;
 }
 

iri_foot/src/foot_alg.cpp~

@@ -25,13 +25,13 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
 
 double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
 {
-	const double PI = 3.14159265359.
-	double numleft = (p1.getX() * p2.getX()) + (p1.getY() * p2.getY()) + (p1.getZ() * p2.getZ());
-	double numright = (p3.getX() * p4.getX()) + (p3.getY() * p4.getY()) + (p3.getZ() * p4.getZ());
-	double denleft = sqrt((p1.getX() * p1.getX()) + (p1.getY() * p1.getY()) + (p1.getZ() * p1.getZ())) * sqrt((p2.getX() * p2.getX()) + (p2.getY() * p2.getY()) + (p2.getZ() * p2.getZ()));
-	double denright = sqrt((p3.getX() * p3.getX()) + (p3.getY() * p3.getY()) + (p3.getZ() * p3.getZ())) * sqrt((p4.getX() * p4.getX()) + (p4.getY() * p4.getY()) + (p4.getZ() * p4.getZ()));
-	double angleft = acos (numleft/denleft);
-	double angright = acos (numright/denright);
-	
+	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;
 }
 

iri_foot/src/foot_alg_node.cpp

 #include "foot_alg_node.h"
+using namespace std;
+#include <string>
 
 FootAlgNode::FootAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
@@ -7,6 +9,7 @@ FootAlgNode::FootAlgNode(void) :
   //this->loop_rate_ = 2;//in [Hz]
 
   // [init publishers]
+  this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
   
   // [init subscribers]
   
@@ -26,13 +29,60 @@ FootAlgNode::~FootAlgNode(void)
 
 void FootAlgNode::mainNodeThread(void)
 {
+string my_var = "";
+try{
+	ros::Time now = ros::Time::now();
+        ros::Duration five_seconds = ros::Duration(5.0);
+	
+	string child_leftfoot("/brix_2/user_1/left_foot");
+	string child_leftknee("/brix_2/user_1/left_knee");
+	string child_rightfoot("/brix_2/user_1/right_foot");
+	string child_rightknee("/brix_2/user_1/right_knee");
+	string parent_cameraframe("/brix_2_camera_frame");
+
+	//left_foot
+	
+	listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);
+	listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
+	leftfoot = transform1.getOrigin();
+	
+	//left_knee
+	
+	listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
+	listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
+	leftknee = trasnform2.getOrigin();
+	
+	//right_foot
+	
+	listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
+	listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
+	rightfoot = trasnform3.getOrigin();
+	
+	//right_knee
+	
+	listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
+	listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
+	rightknee = trasnform4.getOrigin();
+	
+
+
+
+
+}
+
   // [fill msg structures]
+  // Initialize the topic message structure
+  this->myfeet_String_msg_.data = my_var;
+
   
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  // Uncomment the following line to publish the topic message
+  this->myfeet_publisher_.publish(this->myfeet_String_msg_);
+
 }
 
 /*  [subscriber callbacks] */

iri_foot/src/foot_alg_node.cpp~

 #include "foot_alg_node.h"
+using namespace std;
+#include <string>
 
 FootAlgNode::FootAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
@@ -7,6 +9,7 @@ FootAlgNode::FootAlgNode(void) :
   //this->loop_rate_ = 2;//in [Hz]
 
   // [init publishers]
+  this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
   
   // [init subscribers]
   
@@ -26,13 +29,60 @@ FootAlgNode::~FootAlgNode(void)
 
 void FootAlgNode::mainNodeThread(void)
 {
+string my_var = "";
+try{
+	ros::Time now = ros::Time::now();
+        ros::Duration five_seconds = ros::Duration(5.0);
+	
+	string child_leftfoot("/brix_2/user_1/left_foot");
+	string child_leftknee("/brix_2/user_1/left_knee");
+	string child_rightfoot("/brix_2/user_1/right_foot");
+	string child_rightknee("/brix_2/user_1/right_knee");
+	string parent_cameraframe("/brix_2_camera_frame");
+
+	//left_foot
+	
+	listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);
+	listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
+	leftfoot = transform1.getOrigin();
+	
+	//left_knee
+	
+	listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
+	listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
+	leftknee = trasnform2.getOrigin();
+	
+	//right_foot
+	
+	listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
+	listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
+	rightfoot = trasnform3.getOrigin();
+	
+	//right_knee
+	
+	listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
+	listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
+	rightknee = trasnform4.getOrigin();
+	
+
+
+
+
+}
+
   // [fill msg structures]
+  // Initialize the topic message structure
+  this->myfeet_String_msg_.data = my_var;
+
   
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  // Uncomment the following line to publish the topic message
+  this->myfeet_publisher_.publish(this->myfeet_String_msg_);
+
 }
 
 /*  [subscriber callbacks] */