Solved some bugs in the computation of the odometry.

src/ana_odom_alg_node.cpp

@@ -70,18 +70,18 @@ void AnaOdomAlgNode::encoders_callback(const rosaria::Encoders::ConstPtr& msg)
   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/(this->config_.encoder_res*this->config_.gearbox_ratio);
-  disp_right=((double)inc_right)*2.0*3.14159*this->config_.wheel_radius/(this->config_.encoder_res*this->config_.gearbox_ratio);
+  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);
 
   v_left=disp_left/dt.toSec();
   v_right=disp_right/dt.toSec();
   v_trans=(v_left+v_right)/2.0;
-  v_rot=(v_left-v_right)/2.0;
+  v_rot=(v_right-v_left)/(this->config_.wheel_distance);
 
-  theta+=(disp_left-disp_right)/4.0;
+  theta+=(disp_right-disp_left)/(this->config_.wheel_distance*2.0);
   x+=((disp_left+disp_right)/2.0)*cos(theta);
   y+=((disp_left+disp_right)/2.0)*sin(theta);
-  theta+=(disp_left-disp_right)/4.0;
+  theta+=(disp_right-disp_left)/(this->config_.wheel_distance*2.0);
 
   if(this->config_.publish_tf)
   {