Until 28 April

iri_foot/include/foot_alg_node.h → Foot_extended/iri_foot/include/foot_alg_node.h

@@ -48,8 +48,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
 {
   private:
     tf::TransformListener listener;
-    tf::StampedTransform transformrobleftdist;
-    tf::StampedTransform transformrobrightdist;
+    tf::StampedTransform transformrobrightfoot;
+    tf::StampedTransform transformrobleftfoot;
     tf::Vector3 goalleft;
     tf::Vector3 goalright;
 		
@@ -58,6 +58,12 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
     geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
 
     // [subscriber attributes]
+    ros::Subscriber feetextended_subscriber_;
+    void feetextended_callback(const std_msgs::String::ConstPtr& msg);
+    pthread_mutex_t feetextended_mutex_;
+    void feetextended_mutex_enter(void);
+    void feetextended_mutex_exit(void);
+
 
     // [service attributes]
 

iri_foot/include/foot_alg_node.h~ → Foot_extended/iri_foot/include/foot_alg_node.h~

@@ -48,8 +48,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
 {
   private:
     tf::TransformListener listener;
-    tf::StampedTransform transformrobleftdist;
-    tf::StampedTransform transformrobrightdist;
+    tf::StampedTransform transformrobrightfoot;
+    tf::StampedTransform transformrobleftfoot;
     tf::Vector3 goalleft;
     tf::Vector3 goalright;
 		
@@ -58,6 +58,12 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
     geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
 
     // [subscriber attributes]
+    ros::Subscriber feetextended_subscriber_;
+    void feetextended_callback(const std_msgs::String::ConstPtr& msg);
+    pthread_mutex_t feetextended_mutex_;
+    void feetextended_mutex_enter(void);
+    void feetextended_mutex_exit(void);
+
 
     // [service attributes]
 

Foot_extended/iri_foot/src/foot_alg_node.cpp

+#include "foot_alg_node.h"
+using namespace std;
+#include <string>
+
+
+FootAlgNode::FootAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
+{
+  //init class attributes if necessary
+  //this->loop_rate_ = 50;//in [Hz]
+
+  // [init publishers]
+  this->pose_surface_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose_surface", 10);
+  
+  // [init subscribers]
+  this->feetextended_subscriber_ = this->public_node_handle_.subscribe("feetextended", 10, &FootAlgNode::feetextended_callback, this);
+  pthread_mutex_init(&this->feetextended_mutex_,NULL);
+
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+FootAlgNode::~FootAlgNode(void)
+{
+  // [free dynamic memory]
+  pthread_mutex_destroy(&this->feetextended_mutex_);
+}
+
+void FootAlgNode::mainNodeThread(void)
+{
+  string child_rightfoot("/brix_2/user_1/right_foot");
+  string child_leftfoot("/brix_2/user_1/left_foot");
+  string parent_cameraframe("/brix_2_camera_frame");
+  string child_robot("iri_wam_link_base");
+  try{
+    ros::Time zero = ros::Time(0);
+    ros::Time now = ros::Time::now();
+    ros::Duration three_seconds = ros::Duration(3.0);
+    listener.waitForTransform(child_robot, child_rightfoot, zero, three_seconds);
+    listener.waitForTransform(child_robot, child_leftfoot, zero, three_seconds);
+    listener.lookupTransform(child_robot, child_rightfoot, zero,transformrobrightfoot);
+    listener.lookupTransform(child_robot, child_leftfoot, zero,transformrobleftfoot);
+    /*if (msg->data == "Right foot extended")
+    {
+      //The robot has to go to the right foot
+      goalright = transformrobrightfoot.getOrigin();
+    }
+    else if (msg->data == "Left foot extended")
+    {
+      //The robot has to go to the left foot
+      goalleft = transformrobleftfoot.getOrigin();
+    }
+    else
+    {
+      //The robot has to turn back to the initial position
+      
+
+    }
+    */
+
+}
+
+  catch (tf::TransformException &ex) 
+  {
+    ROS_ERROR("%s",ex.what());
+    ros::Duration(1.0).sleep();
+  }
+
+   // [fill msg structures]
+  // Initialize the topic message structure
+  this->pose_surface_PoseStamped_msg_.pose.position.x = goalleft.getX();
+  this->pose_surface_PoseStamped_msg_.pose.position.y = goalleft.getY();
+  this->pose_surface_PoseStamped_msg_.pose.position.z = goalleft.getZ();
+  this->pose_surface_PoseStamped_msg_.pose.orientation.x = 0;
+  this->pose_surface_PoseStamped_msg_.pose.orientation.y = 1;
+  this->pose_surface_PoseStamped_msg_.pose.orientation.z = 0;
+  this->pose_surface_PoseStamped_msg_.pose.orientation.w = 0;  
+  this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time(0);
+  this->pose_surface_PoseStamped_msg_.header.frame_id = "iri_wam_link_base";
+
+  // Initialize the topic message structure
+
+  // Initialize the topic message structure
+
+  // [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->pose_surface_publisher_.publish(this->pose_surface_PoseStamped_msg_);
+
+  // Uncomment the following line to publish the topic message
+
+  // Uncomment the following line to publish the topic message
+  
+
+}
+
+/*  [subscriber callbacks] */
+void FootAlgNode::feetextended_callback(const std_msgs::String::ConstPtr& msg)
+{
+  ROS_INFO("FootAlgNode::feetextended_callback: New Message Received");
+
+  //use appropiate mutex to shared variables if necessary
+  this->alg_.lock();
+  this->feetextended_mutex_enter();
+
+  std::cout << msg->data << std::endl;
+  //unlock previously blocked shared variables
+  this->alg_.unlock();
+  this->feetextended_mutex_exit();
+  if (msg->data == "Right foot extended")
+  {
+     //The robot has to go to the right foot
+     goalright = transformrobrightfoot.getOrigin();
+  }
+  else if (msg->data == "Left foot extended")
+  {
+     //The robot has to go to the left foot
+     goalleft = transformrobleftfoot.getOrigin();
+  }
+    /*else
+    {
+      //The robot has to turn back to the initial position
+      
+
+    }
+    */
+  
+}
+
+void FootAlgNode::feetextended_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->feetextended_mutex_);
+}
+
+void FootAlgNode::feetextended_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->feetextended_mutex_);
+}
+
+/*  [service callbacks] */
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+void FootAlgNode::node_config_update(Config &config, uint32_t level)
+{
+  this->alg_.lock();
+  this->config_=config;
+  this->alg_.unlock();
+}
+
+void FootAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return algorithm_base::main<FootAlgNode>(argc, argv, "foot_alg_node");
+}

Foot_extended/iri_foot/src/foot_alg_node.cpp~

+#include "foot_alg_node.h"
+using namespace std;
+#include <string>
+
+
+FootAlgNode::FootAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
+{
+  //init class attributes if necessary
+  //this->loop_rate_ = 50;//in [Hz]
+
+  // [init publishers]
+  this->pose_surface_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose_surface", 10);
+  
+  // [init subscribers]
+  this->feetextended_subscriber_ = this->public_node_handle_.subscribe("feetextended", 10, &FootAlgNode::feetextended_callback, this);
+  pthread_mutex_init(&this->feetextended_mutex_,NULL);
+
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+FootAlgNode::~FootAlgNode(void)
+{
+  // [free dynamic memory]
+  pthread_mutex_destroy(&this->feetextended_mutex_);
+}
+
+void FootAlgNode::mainNodeThread(void)
+{
+  string child_rightfoot("/brix_2/user_1/right_foot");
+  string child_leftfoot("/brix_2/user_1/left_foot");
+  string parent_cameraframe("/brix_2_camera_frame");
+  string child_robot("iri_wam_link_base");
+  try{
+    ros::Time zero = ros::Time(0);
+    ros::Time now = ros::Time::now();
+    ros::Duration three_seconds = ros::Duration(3.0);
+    listener.waitForTransform(child_robot, child_rightfoot, zero, three_seconds);
+    listener.waitForTransform(child_robot, child_leftfoot, zero, three_seconds);
+    listener.lookupTransform(child_robot, child_rightfoot, zero,transformrobrightfoot);
+    listener.lookupTransform(child_robot, child_leftfoot, zero,transformrobleftfoot);
+    /*if (msg->data == "Right foot extended")
+    {
+      //The robot has to go to the right foot
+      goalright = transformrobrightfoot.getOrigin();
+    }
+    else if (msg->data == "Left foot extended")
+    {
+      //The robot has to go to the left foot
+      goalleft = transformrobleftfoot.getOrigin();
+    }
+    else
+    {
+      //The robot has to turn back to the initial position
+      
+
+    }
+    */
+
+}
+
+  catch (tf::TransformException &ex) 
+  {
+    ROS_ERROR("%s",ex.what());
+    ros::Duration(1.0).sleep();
+  }
+
+   // [fill msg structures]
+  // Initialize the topic message structure
+  this->pose_surface_PoseStamped_msg_.pose.position.x = goalleft.getX();
+  this->pose_surface_PoseStamped_msg_.pose.position.y = goalleft.getY();
+  this->pose_surface_PoseStamped_msg_.pose.position.z = goalleft.getZ();
+  this->pose_surface_PoseStamped_msg_.pose.orientation.x = 0;
+  this->pose_surface_PoseStamped_msg_.pose.orientation.y = 1;
+  this->pose_surface_PoseStamped_msg_.pose.orientation.z = 0;
+  this->pose_surface_PoseStamped_msg_.pose.orientation.w = 0;  
+  this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time(0);
+  this->pose_surface_PoseStamped_msg_.header.frame_id = "iri_wam_link_base";
+
+  // Initialize the topic message structure
+
+  // Initialize the topic message structure
+
+  // [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->pose_surface_publisher_.publish(this->pose_surface_PoseStamped_msg_);
+
+  // Uncomment the following line to publish the topic message
+
+  // Uncomment the following line to publish the topic message
+  
+
+}
+
+/*  [subscriber callbacks] */
+void FootAlgNode::feetextended_callback(const std_msgs::String::ConstPtr& msg)
+{
+  ROS_INFO("FootAlgNode::feetextended_callback: New Message Received");
+
+  //use appropiate mutex to shared variables if necessary
+  this->alg_.lock();
+  this->feetextended_mutex_enter();
+
+  std::cout << msg->data << std::endl;
+  //unlock previously blocked shared variables
+  this->alg_.unlock();
+  this->feetextended_mutex_exit();
+  if (msg->data == "Right foot extended")
+  {
+     //The robot has to go to the right foot
+     goalright = transformrobrightfoot.getOrigin();
+  }
+  else if (msg->data == "Left foot extended")
+  {
+     //The robot has to go to the left foot
+     goalleft = transformrobleftfoot.getOrigin();
+  }
+    /*else
+    {
+      //The robot has to turn back to the initial position
+      
+
+    }
+    */
+  
+}
+
+void FootAlgNode::feetextended_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->feetextended_mutex_);
+}
+
+void FootAlgNode::feetextended_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->feetextended_mutex_);
+}
+
+/*  [service callbacks] */
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+void FootAlgNode::node_config_update(Config &config, uint32_t level)
+{
+  this->alg_.lock();
+  this->config_=config;
+  this->alg_.unlock();
+}
+
+void FootAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return algorithm_base::main<FootAlgNode>(argc, argv, "foot_alg_node");
+}

Foot_extended/iri_foot_extended/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(iri_foot_extended)
+
+## 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/FootExtended.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+ CATKIN_DEPENDS iri_base_algorithm std_msgs
+# ******************************************************************** 
+#      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_extended_alg.cpp src/foot_extended_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} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})

iri_foot_extended/CMakeLists.txt → Foot_extended/iri_foot_extended/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)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -60,7 +60,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm
+ CATKIN_DEPENDS iri_base_algorithm std_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -94,6 +94,7 @@ 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} std_msgs_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
 # ********************************************************************