Use prev gearbox to compute v_rot and theta

src/ana_odom_alg_node.cpp

@@ -56,7 +56,7 @@ void AnaOdomAlgNode::cmd_vel_in_callback(const geometry_msgs::Twist::ConstPtr& m
 
   cmd_vel_out_Twist_msg_=*msg;
   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;
+  //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;
@@ -83,8 +83,10 @@ void AnaOdomAlgNode::encoders_callback(const rosaria::Encoders::ConstPtr& msg)
 
   long int inc_left,inc_right;
   double disp_left,disp_right;
+  double disp_left_old,disp_right_old;
   ros::Duration dt;
   double v_left,v_right,v_trans,v_rot;
+  double v_left_old,v_right_old;
   static double x=0.0,y=0.0,theta=0.0;
 
   geometry_msgs::TransformStamped odom_trans;
@@ -101,15 +103,22 @@ void AnaOdomAlgNode::encoders_callback(const rosaria::Encoders::ConstPtr& msg)
   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);
 
+  disp_left_old=((double)inc_left)*2.0*3.14159*this->config_.wheel_radius/(4.0*this->config_.encoder_res*this->config_.prev_gearbox_ratio*this->config_.transmission_ratio);
+  disp_right_old=((double)inc_right)*2.0*3.14159*this->config_.wheel_radius/(4.0*this->config_.encoder_res*this->config_.prev_gearbox_ratio*this->config_.transmission_ratio);
+
   v_left=disp_left/dt.toSec();
   v_right=disp_right/dt.toSec();
   v_trans=(v_left+v_right)/2.0;
-  v_rot=(v_right-v_left)/(this->config_.wheel_distance);
+  v_left_old=disp_left_old/dt.toSec();
+  v_right_old=disp_right_old/dt.toSec();
+  v_rot=(v_right_old-v_left_old)/(this->config_.wheel_distance);
 
-  theta+=(disp_right-disp_left)/(this->config_.wheel_distance*2.0);
+  theta+=(disp_right_old-disp_left_old)/(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_right-disp_left)/(this->config_.wheel_distance*2.0);
+  theta+=(disp_right_old-disp_left_old)/(this->config_.wheel_distance*2.0);
+
+  //ROS_INFO_STREAM("theta: " << theta);
 
   if(this->config_.publish_tf)
   {