Ask Your Question
4

Running a simulated odometry node

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

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::spin();
  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
    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;

    //ros::spin();
    r.sleep();
   }

}

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

1 Answer

Sort by ยป oldest newest most voted
3

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

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:

//ros::spin();

should be:

ros::spinOnce();

That works for me!

--patrick

edit flag offensive delete link more

Comments

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 imagehitesh ( 2011-03-21 01:05:49 -0500 )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 imageEric Perko ( 2011-03-21 02:25:14 -0500 )edit

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-03-19 22:35:01 -0500

Seen: 3,477 times

Last updated: Mar 20 '11