ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Running a simulated odometry node

asked 2011-03-19 22:35:01 -0600

hitesh gravatar image

Hi all, I'm trying to run an odometry node that is simulated using the ROS navigation stack. I start with values of 0,0,0 for start, and execute a subscriber to listen to /cmd_vel topic, and then use those values as the odometry values. My code is given below:

// Simulated odometry publisher
// Assumes that the robot follows the given velocity commands 
// Basically read in the velocity command values and then transmit them as 
// odometry

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

double vx = 0.0;
double vy = 0.0;
double vth = 0.00;

// Initialize a callback to get velocity values:
void Odocallback( const geometry_msgs::TwistConstPtr& cmd_vel) {

  ROS_INFO( "Velocity_Recieved_by_OdomNode");
  vx = cmd_vel->linear.x;
  vy = cmd_vel->linear.y;
  vth = cmd_vel->angular.z;


int main( int argc, char** argv) {

  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  ros::Subscriber sub = n.subscribe("cmd_vel", 1000, Odocallback);
  tf::TransformBroadcaster odom_broadcaster;

  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  //double vx = 0.0;
  //double vy = 0.0;
  //double vth = 0.0;

  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();
    //vx = cmd_vel.linear.x;
    ///vy = cmd_vel.linear.y;
    //vth = cmd_vel.angular.z;

    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;

    // odometry is 6DOF so 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

    // 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

    last_time = current_time;



In order to change the velocity values, I have made vx, vy and vth as global variables. However, even when the /cmd_vel topic starts publishing data, I do not see any change in the odometry values, nor is my ROS_INFO message being printed. Can anyone pls suggest what I'm doing wrong here?

edit retag flag offensive close merge delete


@hitesh Thanks for this code!

indraneel gravatar image indraneel  ( 2019-11-28 01:42:39 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2011-03-20 02:58:51 -0600

Pi Robot gravatar image

Neat idea! I believe all you need to do is uncomment your second-to-last line and turn it into ros::spinOnce(). In other words, the line:


should be:


That works for me!


edit flag offensive delete link more


Hi Patrick, Thanks for your reply, it works now! But now I'm caught in another problem. I'm using an outdoor video to run nav stack in simulation. I've created a simulated sensor node, that turns detected objects into obstacles in the form of a laser scan. For testing if path planning works, I give the robot a goal. There are two problems I notice: a) The robot always starts to rotate, trying to clear out space, and b) Since the scan is fixed to robot's frame, it rotates along with the robot. I'm pretty sure the parameters are correct, but I'm not sure why it enters recovery immediately. There is no path planned. One way i'm thinking is to use the same odometry values to rotate the image relative to the robot. Any ideas? Thanks!
hitesh gravatar image hitesh  ( 2011-03-21 01:05:49 -0600 )edit
@hitesh: That might be better asked as a new question where you have enough space to detail your setup (and maybe include some of the relevant code/config files). Also, make sure to "accept" whichever answer solves your question so that others in the future can easily find the fix.
Eric Perko gravatar image Eric Perko  ( 2011-03-21 02:25:14 -0600 )edit

Question Tools


Asked: 2011-03-19 22:35:01 -0600

Seen: 4,039 times

Last updated: Mar 20 '11