Hi all,
I have a problem to compute odometry from wheel encoders, I mean, i don't have a real problem, I just don't understand a step.
I get the ticks from the encoders (they are about 2626 ticks for a full wheel rotation) and I perform the odometry calculation in the following way(of course this is in a loop...i paste just the important lines about the odometry calculation)
current_time = ros::Time::now();
double DistancePerCount = (3.14159265 * 0.13) / 2626;
double lengthBetweenTwoWheels = 0.25;
// extract the wheel velocities from the tick signals count
deltaLeft = tick_x - _PreviousLeftEncoderCounts;
deltaRight = tick_y - _PreviousRightEncoderCounts;
omega_left = (deltaLeft * DistancePerCount) / (current_time - last_time).toSec();
omega_right = (deltaRight * DistancePerCount) / (current_time - last_time).toSec();
v_left = omega_left * 0.065; //radius
v_right = omega_right * 0.065;
vx = ((v_right + v_left) / 2)*10;
vy = 0;
vth = ((v_right - v_left)/lengthBetweenTwoWheels)*10;
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th)) * dt;
double delta_y = (vx * sin(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
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);
// Odometry message
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 = vy;
odom.twist.twist.angular.z = vth;
// publish the message
odom_pub.publish(odom);
_PreviousLeftEncoderCounts = tick_x;
_PreviousRightEncoderCounts = tick_y;
last_time = current_time;
The thing that I don't understand is why i have to multiply vx and vth for 10 (it is a value that i set randomly and seems to work) to make the odometry consistent with the movements of the robot (if i dont multply, the odometry will change very slowly compared to what the robot does in terms of meters done).
Maybe I'm doing some errors calculating the velocities.
An help would be very very appreciated, thank you.
Originally posted by dottant on ROS Answers with karma: 185 on 2016-08-12
Post score: 3
Original comments
Comment by Ayush Sharma on 2017-04-29:
My i please know the source from where i can implement the same? I am also trying to retrieve the encoder values but not able to find anything useful so far. Regards
Comment by akay97 on 2017-05-20:
Can u please share the whole code for encoders plzz. I am really stuck and can't seem to find any solution ...
Comment by PG_GrantDare on 2018-11-13:
Could I ask where you are getting the equations for your v and theta?
Comment by sajal on 2019-05-30:
@dottant why have you multiplied vx and vth by 10?
Comment by dottant on 2019-06-03:
This topic is old, there's no need to perform that, the formula was wrong cause I had to fix some hardware issue related to the IMU and I completely changed the code introducing a PID control for the velocities commands to the robot which now gives me smooth values and a quite perfect odometry (of course with a little slippage whose value can be seen only after a long time). Just remove the *10 and be sure to have perfect values from the IMU.