Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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

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