Ask Your Question
2

Gathering Wheel Encoder data and publishing on the /odom topic

asked 2011-11-16 08:37:01 -0500

D_mangus gravatar image

So far I have rosserial up and running. It's feeding Vector3's back to my main computer where Vector3.x = right encoder ticks and Vector3.y is left encoder ticks. I'm getting the data back on that topic and I can view it using "rostopic echo /wheel_encoder" it seems to be sending data back perfectly fine. Now I have created the node below to receive the data and translate the ticks into x,y translation and /odom data using the guide from the navigation stack. For some reason my /odom data is not changing at all. I'm assuming it has to do with the global variables I have and the callback function but I can't seem to track it down. Any insight would be helpful.

#include "ros/ros.h"
#include <geometry_msgs/Vector3.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

long _PreviousLeftEncoderCounts = 0;
long _PreviousRightEncoderCounts = 0;
ros::Time current_time_encoder, last_time_encoder;
double DistancePerCount = (3.14159265 * 0.1524) / 64000;

double x;
double y;
double th;

double vx;
double vy;
double vth;
double deltaLeft;
double deltaRight;

void WheelCallback(const geometry_msgs::Vector3::ConstPtr& ticks)
{

  current_time_encoder = ros::Time::now();

  deltaLeft = ticks->x - _PreviousLeftEncoderCounts;
  deltaRight = ticks->y - _PreviousRightEncoderCounts;

  vx = deltaLeft * DistancePerCount; // (current_time_encoder - last_time_encoder).toSec();
  vy = deltaRight * DistancePerCount; // (current_time_encoder - last_time_encoder).toSec();


  _PreviousLeftEncoderCounts = ticks->x;
  _PreviousRightEncoderCounts = ticks->y;
  last_time_encoder = current_time_encoder;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "odometry_publisher");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("wheel_encoder", 100, WheelCallback);
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);   
  tf::TransformBroadcaster odom_broadcaster;


  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();


  ros::Rate r(1.0);
  while(n.ok()){

    current_time = ros::Time::now();

    //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;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    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 = vy;
    odom.twist.twist.angular.z = vth;

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

    last_time = current_time;
    r.sleep();
  }
}
edit retag flag offensive close merge delete

Comments

Hello I am trying to do something similar,I am using US digital encoders and its giving out hexa decimal values. I would like to know what tick means in your post?. Is it just the change in value of the encoder or one complete cycle?

metal gravatar image metal  ( 2012-07-26 10:01:48 -0500 )edit

Your vth component is undefined.

Cerin gravatar image Cerin  ( 2016-12-11 17:28:55 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
6

answered 2011-11-16 08:56:14 -0500

tfoote gravatar image

Your wheel callback is not going to be called because you are not letting ROS spin. See the roscpp overview for more details.

The simple solution is to add ros::SpinOnce(); before your r.sleep().

I also highly suggest that you review the roscpp tutorials which covers this sort of thing closely.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2011-11-16 08:37:01 -0500

Seen: 7,844 times

Last updated: Nov 16 '11