Added a subscriber of the cmd_vel topic to generate a new one with the gearbox correction applied.

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm nav_msgs rosaria)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm geometry_msgs nav_msgs rosaria)
 
 ## 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 nav_msgs rosaria
+ CATKIN_DEPENDS iri_base_algorithm geometry_msgs nav_msgs rosaria
 # ******************************************************************** 
 #      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} geometry_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} nav_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} rosaria_generate_messages_cpp)
 # ******************************************************************** 

include/ana_odom_alg_node.h

@@ -29,6 +29,7 @@
 #include "ana_odom_alg.h"
 
 // [publisher subscriber headers]
+#include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
 #include <rosaria/Encoders.h>
 
@@ -45,10 +46,19 @@ class AnaOdomAlgNode : public algorithm_base::IriBaseAlgorithm<AnaOdomAlgorithm>
 {
   private:
     // [publisher attributes]
+    ros::Publisher cmd_vel_out_publisher_;
+    geometry_msgs::Twist cmd_vel_out_Twist_msg_;
+
     ros::Publisher odom_publisher_;
     nav_msgs::Odometry odom_Odometry_msg_;
 
     // [subscriber attributes]
+    ros::Subscriber cmd_vel_in_subscriber_;
+    void cmd_vel_in_callback(const geometry_msgs::Twist::ConstPtr& msg);
+    pthread_mutex_t cmd_vel_in_mutex_;
+    void cmd_vel_in_mutex_enter(void);
+    void cmd_vel_in_mutex_exit(void);
+
     ros::Subscriber encoders_subscriber_;
     void encoders_callback(const rosaria::Encoders::ConstPtr& msg);
     pthread_mutex_t encoders_mutex_;

package.xml

@@ -50,6 +50,7 @@
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_algorithm</build_depend>
+  <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>rosaria</depend>
   <build_export_depend>iri_base_algorithm</build_export_depend>

src/ana_odom_alg_node.cpp

@@ -7,9 +7,13 @@ AnaOdomAlgNode::AnaOdomAlgNode(void) :
   //this->loop_rate_ = 2;//in [Hz]
 
   // [init publishers]
+  this->cmd_vel_out_publisher_ = this->private_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_out", 1);
   this->odom_publisher_ = this->private_node_handle_.advertise<nav_msgs::Odometry>("odom", 1);
   
   // [init subscribers]
+  this->cmd_vel_in_subscriber_ = this->private_node_handle_.subscribe("cmd_vel_in", 1, &AnaOdomAlgNode::cmd_vel_in_callback, this);
+  pthread_mutex_init(&this->cmd_vel_in_mutex_,NULL);
+
   this->encoders_subscriber_ = this->private_node_handle_.subscribe("encoders", 1, &AnaOdomAlgNode::encoders_callback, this);
   pthread_mutex_init(&this->encoders_mutex_,NULL);
 
@@ -26,27 +30,51 @@ AnaOdomAlgNode::AnaOdomAlgNode(void) :
 AnaOdomAlgNode::~AnaOdomAlgNode(void)
 {
   // [free dynamic memory]
+  pthread_mutex_destroy(&this->cmd_vel_in_mutex_);
   pthread_mutex_destroy(&this->encoders_mutex_);
 }
 
 void AnaOdomAlgNode::mainNodeThread(void)
 {
   // [fill msg structures]
-  // Initialize the topic message structure
-  //this->odom_Odometry_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->odom_publisher_.publish(this->odom_Odometry_msg_);
-
 }
 
 /*  [subscriber callbacks] */
+void AnaOdomAlgNode::cmd_vel_in_callback(const geometry_msgs::Twist::ConstPtr& msg)
+{
+  ROS_INFO("AnaOdomAlgNode::cmd_vel_in_callback: New Message Received");
+
+  //use appropiate mutex to shared variables if necessary
+  //this->alg_.lock();
+  this->cmd_vel_in_mutex_enter();
+
+  cmd_vel_out_Twist_msg_=*msg;
+  cmd_vel_out_Twist_msg_.linear.x*=this->config_.prev_gearbox_ratio/this->config_.current_gearbox_ratio;
+  cmd_vel_out_Twist_msg_.angular.z*=this->config_.prev_gearbox_ratio/this->config_.current_gearbox_ratio;
+  this->cmd_vel_out_publisher_.publish(this->cmd_vel_out_Twist_msg_);
+
+  //std::cout << msg->data << std::endl;
+  //unlock previously blocked shared variables
+  //this->alg_.unlock();
+  this->cmd_vel_in_mutex_exit();
+}
+
+void AnaOdomAlgNode::cmd_vel_in_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->cmd_vel_in_mutex_);
+}
+
+void AnaOdomAlgNode::cmd_vel_in_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->cmd_vel_in_mutex_);
+}
+
 void AnaOdomAlgNode::encoders_callback(const rosaria::Encoders::ConstPtr& msg)
 {
   static long int prev_left_enc=0;