Added the transmission ratio parameter.

Completed and tested the computation of the odometry and the command vel.

cfg/AnaOdom.cfg

@@ -41,6 +41,7 @@ gen = ParameterGenerator()
 gen.add("encoder_res",             int_t,     0,                               "Encoder resolution in cpr",      100,      1,    10000)
 gen.add("current_gearbox_ratio",   double_t,  0,                               "Current gearbox ratio",          38.3,     1.0,  1000.0)
 gen.add("prev_gearbox_ratio",      double_t,  0,                               "Previous gearbox ratio",         65.5,     1.0,  1000.0)
+gen.add("transmission_ratio",      double_t,  0,                               "Transmission ratio",             1.33,     1.0,  1000.0)
 gen.add("wheel_radius",            double_t,  0,                               "Radius of the wheel",            0.11,     0.01, 1.0)
 gen.add("wheel_distance",          double_t,  0,                               "Disatance between wheels",       0.4,      0.1,  10.0)
 gen.add("publish_tf",              bool_t,    0,                               "Whether to publish the TF transform",True)

config/params.yaml

 encoder_res: 100
 current_gearbox_ratio: 38.3
 prev_gearbox_ratio: 65.5
+transmission_ratio: 1.333
 wheel_radius: 0.11
 wheel_distance: 0.4
 publish_tf: True

launch/ana_odom.launch

@@ -19,4 +19,4 @@
     
   </group>
 
-</launch>
\ No newline at end of file
+</launch>

src/ana_odom_alg_node.cpp

@@ -48,15 +48,15 @@ void AnaOdomAlgNode::mainNodeThread(void)
 /*  [subscriber callbacks] */
 void AnaOdomAlgNode::cmd_vel_in_callback(const geometry_msgs::Twist::ConstPtr& msg)
 {
-  ROS_INFO("AnaOdomAlgNode::cmd_vel_in_callback: New Message Received");
+//  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;
+  cmd_vel_out_Twist_msg_.linear.x*=this->config_.current_gearbox_ratio/this->config_.prev_gearbox_ratio;
+  cmd_vel_out_Twist_msg_.angular.z*=this->config_.current_gearbox_ratio/this->config_.prev_gearbox_ratio;
   this->cmd_vel_out_publisher_.publish(this->cmd_vel_out_Twist_msg_);
 
   //std::cout << msg->data << std::endl;
@@ -89,17 +89,17 @@ void AnaOdomAlgNode::encoders_callback(const rosaria::Encoders::ConstPtr& msg)
 
   geometry_msgs::TransformStamped odom_trans;
 
-  ROS_INFO("AnaOdomAlgNode::encoders_callback: New Message Received");
+//  ROS_INFO("AnaOdomAlgNode::encoders_callback: New Message Received");
 
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();
   this->encoders_mutex_enter();
-  inc_left=msg->left_encoder-prev_left_enc;
-  inc_right=msg->right_encoder-prev_right_enc;
+  inc_left=(msg->left_encoder-prev_left_enc);
+  inc_right=(msg->right_encoder-prev_right_enc);
   dt=msg->header.stamp-prev_time;
 
-  disp_left=((double)inc_left)*2.0*3.14159*this->config_.wheel_radius/(4.0*this->config_.encoder_res*this->config_.prev_gearbox_ratio*3.37);
-  disp_right=((double)inc_right)*2.0*3.14159*this->config_.wheel_radius/(4.0*this->config_.encoder_res*this->config_.prev_gearbox_ratio*3.37);
+  disp_left=((double)inc_left)*2.0*3.14159*this->config_.wheel_radius/(4.0*this->config_.encoder_res*this->config_.current_gearbox_ratio*this->config_.transmission_ratio);
+  disp_right=((double)inc_right)*2.0*3.14159*this->config_.wheel_radius/(4.0*this->config_.encoder_res*this->config_.current_gearbox_ratio*this->config_.transmission_ratio);
 
   v_left=disp_left/dt.toSec();
   v_right=disp_right/dt.toSec();