Ask Your Question
0

Odometry message errors

asked 2016-03-23 05:12:07 -0600

Bert gravatar image

Hi!

With examples I found online I made an odometry publisher that generates odometry data (for gmapping) from encoder ticks. But When the robot is going straight forward, the odometry data gives the same translation in x en y direction. Where are my mistakes? Here is the full publisher:

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

long _PreviousLeftEncoderCounts = 0;
long _PreviousRightEncoderCounts = 0;
ros::Time current_time_encoder, last_time_encoder;
//double DistancePerCount = (3.14159265 * 0.1524) / 200;
double DistancePerCount=1;
double x;
double y;
double th=0;
double W;
double V;
double vx;
double vy;
double vth;
double deltaLeft;
double deltaRight;


double d=50; //afstand tussen de wielen

int ticks_links;
int ticks_rechts;


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

void WheelCallback(const mastering_ros_demo_pkg::encoder_msg::ConstPtr& msg)
{
  //tijd van de meting 
  current_time_encoder = ros::Time::now();

 //verschil in ticks tov vorige berekening 
  deltaLeft = msg->ticks_links - _PreviousLeftEncoderCounts;
  deltaRight = msg->ticks_rechts - _PreviousRightEncoderCounts;

 //snelheid links (x) en rechts (y)
  vx = deltaLeft * DistancePerCount / (current_time_encoder - last_time_encoder).toSec();
  vy = deltaRight * DistancePerCount / (current_time_encoder - last_time_encoder).toSec();

 //als snelheid links = snelheid rechts -> snelheid = snelheid links en hoeksnelheid = 0
if (vx == vy)
{
    V = vx;
    W = 0;
}


else
{
 // Assuming the robot is rotating about point A   
 // W = vel_left/r = vel_right/(r + d), see the image below for r and d
        double r = (vx * d) / (vy - vx); // Anti Clockwise is positive
        W = vx/r; // Rotational velocity of the robot
        V = W * (r + d/2); // Translation velocity of the robot
}

 //vth = hoeksnelheid
vth= W;

 //aantal ticks onthouden 
  _PreviousLeftEncoderCounts = msg->ticks_links;
  _PreviousRightEncoderCounts = msg->ticks_rechts;

 //tijd van de laatste meting onthouden
  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("tick_topic", 100, WheelCallback);
//ros::Subscriber sub1 = n.subscribe("tick_topic", 100, ticks_topic_callback);
  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(20.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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-03-23 06:41:05 -0600

mgruhler gravatar image

This is because your set vx and vy in the callback to be the velocities of the right and left wheel, respectively. This is what you use in your main.

It seems to me you are having a differntial drive robot, so you would need to use V for twist.linear.x, twist.linear.y should be zero, and twist.angluar.z should be W.

Looks to me you followed this tutorial in the publishing part. Note the vx and vy here are the velocities of the robot with respect to its `base_link.

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

1 follower

Stats

Asked: 2016-03-23 05:12:07 -0600

Seen: 165 times

Last updated: Mar 23 '16