How to get robot position in map?

Question

I'm having problems overlaying a trajectory onto an occupancy grid plotted using Matplotlib with the correct alignment.

My question is how do I get the robot position in the map frame so that when I plot both using Matplotlib the robot trajectory is in the right place?

Note that I am using cartographer_ros.

EDIT: Here's what I've tried to get the robot pose so far. It allows me to plot the trajectory but it doesn't have the same rotation as the map and is not aligned to it, although the XY alignment may well be a plotting issue rather than to do with ROS. It only produces sensible looking results when required_position = "odom" and frame = "base_link", which does not make sense to me because I would of thought that it should be like: required_position = "base_link" and frame = "map".

def get_map_pose(self, required_position, frame):
    tf_listener = tf.TransformListener()
    tf_listener.waitForTransform(required_position, frame, rospy.Time(), rospy.Duration(5.0))
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now()
            tf_listener.waitForTransform(required_position, frame, now, rospy.Duration(5.0))
            (t, r) = tf_listener.lookupTransform(required_position, frame, now)
            euler_r = list(tf.transformations.euler_from_quaternion(r))
            return t, euler_r
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

---

Originally posted by Py on ROS Answers with karma: 501 on 2020-06-25

Post score: 0

---

Original comments

Comment by billy on 2020-06-28:
Does robot-pose-publisher not do whet you need? http://wiki.ros.org/robot_pose_publisher

Comment by Py on 2020-06-29:
This seems to only be available for Kinetic, I'm using Melodic.

Answer 1

Like this: https://github.com/ros-planning/navigation2/blob/f510bbc4ed42274ce13bcc756758113e70b6145e/nav2_util/src/robot_utils.cpp

Use TF2 to get the pose from the map frame to your base frame, whatever you like.

---

Originally posted by stevemacenski with karma: 8272 on 2020-06-25

This answer was NOT ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Py on 2020-06-26:
Thanks for the reply. At a glance, it looks like this is for ROS2. Can I use it with ROS Melodic? I'll update my question with something I've tried using tf.TransformListener(), it'd be good to hear what you think.

Comment by stevemacenski on 2020-06-26:
Just update with the ROS1 version of things, they're not dissimilar.

Comment by Py on 2020-06-29:
I'm not entirely sure what you mean. Could you point me to the ROS1 equivalent please? I wondered if it was something like the code I've added to my question?

Answer 2

I guess you already have the map subscribed as an OccupancyGrid: from nav_msgs.msg import OccupancyGrid

If so, for me, I found that the ROS odom x,y axis needed to be transposed to y,x of the map OccupancyGrid (i.e. odom x {left to right in RVIZ} = map y, while odom y {down to up in RVIZ} = map x). Also you need to translate the map-origin with its x,y = 0,0 being at its bottom left hand corner; using the OccupancyGrid.info meta-data.

Also with tf, you may find that applying the odom to map transform, is easier using: from tf2_geometry_msgs import PoseStamped, do_transform_pose

• e.g. like: in_map_pose = do_transform_pose(msg.pose, trans)

---

Originally posted by Slider with karma: 51 on 2020-10-30

This answer was NOT ACCEPTED on the original site

Post score: 1