0
$\begingroup$

Rosanswers logo

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();
  }
}

Originally posted by D_mangus on ROS Answers with karma: 121 on 2011-11-16

Post score: 2


Original comments

Comment by metal on 2012-07-26:
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?

Comment by Cerin on 2016-12-11:
Your vth component is undefined.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by tfoote with karma: 58457 on 2011-11-16

This answer was ACCEPTED on the original site

Post score: 6

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.