Revision history [back]

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
}
{
//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. Is this not the right way to get the current pose of a position model in stageros? Do I need to do something else?

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
}
{
//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. 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?