Ask Your Question
0

Is this the right way to get odometry information in stageros?

asked 2012-08-17 00:59:05 -0600

Clefairy gravatar image

updated 2012-08-17 01:00:26 -0600

I'm simulating robots in stageros and I want to use stageros' odom topic to get the current position of the robot. This is how my code looks:

}
    // In a constructor
    RobotOdometry_sub = n.subscribe<nav_msgs::Odometry>("robot_0/odom",1000,&Robot::ReceiveOdometry,this);
}
void Robot::ReceiveOdometry(nav_msgs::Odometry msg)
{
    //This is the call back function to process odometry messages coming from Stage.    
    px = initialX + msg.pose.pose.position.x;
    py = initialY + msg.pose.pose.position.y;
    ptheta = initialTheta + angles::normalize_angle_positive(asin(msg.pose.pose.orientation.z) * 2);

    while (ptheta > 2 * M_PI)
    {
        ptheta -= 2 * M_PI;
    }
    while (ptheta < 0)
    {
        ptheta += 2 * M_PI;
    }
}

For some reason though the px and py values are wrong whenever I check them and they don't match up with the position of the robot in the stageros world. Is this not the right way to get the current pose of a position model in stageros? Do I need to do something else?

Please help.

edit retag flag offensive close merge delete

Comments

How do the values px and py look like and what should they be? Did you try with initialX and initialY set to 0? Btw. for calculating theta, you should probably use tf::getYaw.

Lorenz gravatar imageLorenz ( 2012-08-17 01:19:46 -0600 )edit

px and py are 9.9 and 10.1 when they are 5.7 and 12.4 in the world. initialX and initialY were set to 15 and 20. Doesn't seem to work when it's set to 0.

Clefairy gravatar imageClefairy ( 2012-08-17 02:49:20 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-08-17 02:31:35 -0600

michikarg gravatar image

The /odom topic usually specifies the estimated position of the robot relative to its starting position and not relative to the map. If you want to obtain the position of your robot in the map, you will have to listen on a topic of your (fake) localization (give you are running localization). I don't know for Stage, but usually this should e.g. be /amcl_pose (if you use ROS amcl localization).

Alternatively you can query TF for a transform between the /base_link frame of your robot and the /map-frame (again assuming you run a localization).

edit flag offensive delete link more

Comments

ah I see, you are somehow already considering the Initial-pose of the robot... but maybe it would make sense anyway to use the tf transforms instead of the Odometry message...

michikarg gravatar imagemichikarg ( 2012-08-17 02:35:54 -0600 )edit

I'm not that familiar with localization. I've read the amcl ROS wiki and tried subscribing to amcl_pose before but it didn't seem to work because when I type "rosrun amcl amcl scan:=base_scan", it just says "Request for map failed; trying again...".

Clefairy gravatar imageClefairy ( 2012-08-17 02:39:11 -0600 )edit

I couldn't figure it out because there's so little documentation other than the ROS wiki. Is there a procedure I'm supposed to follow to get it to work?

Clefairy gravatar imageClefairy ( 2012-08-17 02:40:12 -0600 )edit

AMCL needs a map of your environment i.e. a map-server must be running and a map loaded, otherwise localization will not work... But I'm not sure if your stage-environment includes a 2D-gridmap since i only used stageros 2 years ago...

michikarg gravatar imagemichikarg ( 2012-08-17 02:50:27 -0600 )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: 2012-08-17 00:59:05 -0600

Seen: 1,301 times

Last updated: Aug 17 '12