ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org

# Running a simulated odometry node

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 <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::Subscriber sub = n.subscribe("cmd_vel", 1000, Odocallback);

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.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;

// 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.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 close merge delete

Sort by » oldest newest most voted

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

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!
( 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.
( 2011-03-21 02:25:14 -0500 )edit