Revision history [back]

The problem is in the way you calculate the odometry. You mention that you took the code from the example here. But that code is for a Holonomic robot (it can move independently on the Y direction), but your robot is non-holonomic.

Based on the code mentioned before, here is how you should calculate the odometry of your robot:

//compute odometry in a typical way given the velocities of the robot
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) /* - vy * sin(th)  */ ) * dt; //for non-holonomic robots vy*sin(th)=0
double delta_y = (vx * sin(th) /* + vy * cos(th)  */ ) * dt; //for non-holonomic robots vy*cos(th)=0
double delta_th = vth * dt;

x += delta_x;
y += delta_y;
th += delta_th;

//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";

odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;

//send the transform
odom_broadcaster.sendTransform(odom_trans);

//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";

//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;

//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = 0; // for non-holonomic robots this is always 0
odom.twist.twist.angular.z = vth;

//publish the message
odom_pub.publish(odom);

I hope this solves your issue