How to calculate odometry using encoders ?
I have tried all the solutions but can't seem to make it work, I can see the odometry in rviz with respect to the /odom frame, and see the tf go around in circles but it doesn't move with respect to the base_link frame. When I try to plot it in MATLAB, I get this plot, this is the robot going straight.
Complete code:Odometry.cpp 
void update_velocities(
double dt)
if (dt > 0) {
int curr_encoder_count_left =
current_encoder_count_left;
int curr_encoder_count_right =
current_encoder_count_right;
{
double cos_current = cos(theta);
double sin_current = sin(theta);
// relying on the compiler for wraparound here
int dist_left_counts =
curr_encoder_count_left -
previous_encoder_count_left;
int dist_right_counts =
curr_encoder_count_right -
previous_encoder_count_right;
if (dist_left_counts != dist_right_counts) {
// convert from counts to standard units
double dist_left_mm =
(double)dist_left_counts /
left_encoder_counts_per_mm;
double dist_right_mm =
(double)dist_right_counts /
right_encoder_counts_per_mm;
double right_left = dist_right_mm - dist_left_mm;
double a =
wheelbase_mm * (dist_right_mm + dist_left_mm) *
0.5 / right_left;
double fraction = right_left / wheelbase_mm;
const double mm_to_m = 1.0 / 1000;
vx = a * (sin(fraction + theta) - sin_current) *
mm_to_m;
vy = -a * (cos(fraction + theta) - cos_current) *
mm_to_m;
vtheta = fraction;
// ROS_INFO("pose %.3f %.3f orientation %.3f",
// (float)x, (float)y, (float)theta);
ROS_DEBUG("Encoders %.3f %.3f",
dist_left_mm, dist_right_mm);
}
else {
vx=vy=vtheta=0;
}
// store previous values
previous_encoder_count_left = curr_encoder_count_left;
previous_encoder_count_right = curr_encoder_count_right;
}
}
ros::init(argc, argv, "phidgets_odometry");
ros::NodeHandle n;
ros::NodeHandle nh("~");
std::string name = "odom";
nh.getParam("name", name);
std::string reset_topic = "odometry/reset";
nh.getParam("reset_topic", reset_topic);
nh.getParam("rotation", rotation_offset);
n.setParam(reset_topic, false);
ros::Publisher odom_pub =
n.advertise<nav_msgs::Odometry>(name, 50);
tf::TransformBroadcaster odom_broadcaster;
// get encoder indexes
encoder_index_left = -1;
nh.getParam("encoderindexleft", encoder_index_left);
encoder_index_right = -1;
nh.getParam("encoderindexright", encoder_index_right);
int direction = -1;
nh.getParam("encoderdirectionleft", direction);
if (direction!=-1) encoder_direction_left = 1;
direction=1;
nh.getParam("encoderdirectionright",
encoder_direction_right);
if (direction!=1) encoder_direction_right = -1;
// connect to the encoders
bool subscribed_to_encoders = false;
if ((encoder_index_left > 0) &&
(encoder_index_right > 0) &&
(encoder_index_left!=encoder_index_right)) {
subscribed_to_encoders =
subscribe_to_encoders_by_index();
}
else {
subscribed_to_encoders = subscribe_to_encoders();
}
if (subscribed_to_encoders) {
std::string base_link = "base_link";
nh.getParam("base_link", base_link);
std::string frame_id = "odom";
nh.getParam("frame_id", frame_id);
left_encoder_counts_per_mm = 0;
right_encoder_counts_per_mm = 0;
nh.getParam("countspermmleft",
left_encoder_counts_per_mm);
if (left_encoder_counts_per_mm < 1) {
left_encoder_counts_per_mm = 1000;
}
nh.getParam("countspermmright",
right_encoder_counts_per_mm);
if (right_encoder_counts_per_mm < 1) {
right_encoder_counts_per_mm = 1000;
}
wheelbase_mm = 0;
nh.getParam("wheelbase", wheelbase_mm);
if (wheelbase_mm < 1) wheelbase_mm = 400;
nh.getParam("verbose", verbose);
nh.setParam("verbose", false);
int frequency = 25;
nh.getParam("frequency", frequency);
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
initialised = true;
ros::Rate update_rate(frequency);
while(ros::ok()){
// reset the pose
bool reset = false;
n.getParam(reset_topic, reset);
if (reset) {
x = 0;
y = 0;
theta = 0;
vx = 0;
vy = 0;
vtheta = 0;
n.setParam(reset_topic, false);
start_encoder_count_left = 0;
start_encoder_count_right = 0;
}
current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();
// update the velocity estimate based upon
// encoder values
update_velocities(dt);
// compute odometry in a typical way given
// the velocities of the robot
x += vx;
y += vy;
theta += vtheta;
// since all odometry is 6DOF we'll need a
// quaternion created from yaw
geometry_msgs::Quaternion odom_quat =
tf::createQuaternionMsgFromYaw(theta);
// first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = frame_id;
odom_trans.child_frame_id = base_link;
odom_trans.transform.translation.x = x;
odom_trans.transform ...add a comment
