ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to get robot position in map?

asked 2020-06-25 11:36:46 -0500

Py gravatar image

updated 2020-06-26 03:19:55 -0500

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
edit retag flag offensive close merge delete

Comments

Does robot-pose-publisher not do whet you need? http://wiki.ros.org/robot_pose_publisher

billy gravatar image billy  ( 2020-06-28 00:51:42 -0500 )edit

This seems to only be available for Kinetic, I'm using Melodic.

Py gravatar image Py  ( 2020-06-29 05:34:47 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2020-10-30 10:00:22 -0500

updated 2020-11-01 11:50:49 -0500

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)
edit flag offensive delete link more
1

answered 2020-06-25 14:28:45 -0500

Like this: https://github.com/ros-planning/navig...

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

edit flag offensive delete link more

Comments

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.

Py gravatar image Py  ( 2020-06-26 03:08:07 -0500 )edit

Just update with the ROS1 version of things, they're not dissimilar.

stevemacenski gravatar image stevemacenski  ( 2020-06-26 13:23:42 -0500 )edit

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?

Py gravatar image Py  ( 2020-06-29 05:31:00 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2020-06-25 11:36:46 -0500

Seen: 2,865 times

Last updated: Nov 01 '20