Get robot position und orientation (in degree) from TF
Hi,
I want to get the Position (x, y) and the orientation from my robot. The position should be relative to the map TF. Therefore I used a TransformListener:
tf::StampedTransform transform_bot;
tf::TransformListener listener;
listener.lookupTransform("/map", "/base_footprint", ros::Time(0), transform_bot);
bot_x = transform_bot.getOrigin().x();
bot_y = transform_bot.getOrigin().y();
double roll, pitch, yaw;
transform_bot.getBasis().getRPY(roll, pitch, yaw);
bot_dir = yaw * 180.0 / PI;
ROS_INFO("Bot X: %f", bot_x);
ROS_INFO("Bot Y: %f", bot_y);
ROS_INFO("Bot Z: %f", bot_dir);
So far I was not able to get good result with this solution...
Do you have any ideas, how to solve the problem?
Thank you, Sebastian