Is this the right way to get odometry information in stageros?
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.
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.
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.