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

Robot coordinates in map

asked 2015-03-21 04:40:27 -0600

RND gravatar image


I am developing an exploration algorithm based on frontier exploration. In my code, I am subscribing to the topic /map and then I am processing the data[] vector that I obtain from that topic messages. On subscribing to /map I get a 4000 x 4000 pixels map (i.e. height = width = 4000 pixels). Each pixel has its own x and y coordinate and I am using the pixel values (0, +100 or -1) in order to extract the (x,y) coordinates of the free cells (no obstacles) that are adjacent to unknown cells (cells not yet explored).

Now I need to get the robot location in (x,y) coordinates within that map. This means that I must have some (x,y) coordinates within the 4000 x 4000 map, i.e. within the vector data[] that is returned by /map. I have tried to listen in to the transform between /map and /odom but once I get the robot pose with respect to the map (i.e. according to the transform), it is still not making a lot of sense. I am following this code and my robot pose output is of the following nature:

  x: -9.87982119798
  y: 16.8167273361
  z: 0.0
  x: 0.0
  y: 0.0
  z: 0.627633768897
  w: 0.778508736072

For me, this is not making much sense because I would need some (x,y) value that corresponds to some (x,y) coordinate in the map. This image might help you understand what I mean and the kind of result I'm after.

Thank you in advance.

edit retag flag offensive close merge delete


Hi sobot. What i did was I iterated through every value in the data[] vector and used the index used in the map_saver code in order to extract the (x,y) coordinates. If the value in data[] is 0 -> free cell, if 100 -> occupied, else it is unknown. The key is to have two nested loops, one for y and x

RND gravatar image RND  ( 2015-05-04 02:56:35 -0600 )edit

For the record, the outer loop goes through the y coordinates and the inner loop through the x coordinates. Then to index data[] use index i = x + ( - y -1)*

RND gravatar image RND  ( 2015-05-04 02:57:59 -0600 )edit

Thanks so much for your reply. i got the idea behind it. though since i'm quite new to programming its a bit complicated for me to write it, do you have the code snip uploaded somewhere so i can take look at it and adapt it to my application? PS: i got code for Yamauchi frontier explr if u stil need

sobot gravatar image sobot  ( 2015-05-04 03:52:38 -0600 )edit

Here's a snippet of my work. Sorry could not share it all but it's still works in progress. Can you share the code you got for Yamauchi frontier exploration please? Would like to have a look. Thanks!

RND gravatar image RND  ( 2015-05-04 04:16:09 -0600 )edit

Here's the Planner function of the Yamauchi based frontier proj. that i have Planner, hope that's answers your question. i'll take a look at ur code snip... thanks for that mate.

sobot gravatar image sobot  ( 2015-05-04 05:56:20 -0600 )edit

so you are incorporating the path planning function with the frontier exploration algorithm all in one?

RND gravatar image RND  ( 2015-05-04 06:24:02 -0600 )edit

finding target and planning happens all in one node. you maybe interested in another part of the code too. here is the link to the target finding that happens under a mapCallback function. got the snips to help my navigation cause though

sobot gravatar image sobot  ( 2015-05-04 07:36:33 -0600 )edit

I was going to use the move_base package for navigation. How does that sound?

RND gravatar image RND  ( 2015-05-04 07:55:19 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2015-03-21 07:36:35 -0600

lucasw gravatar image

updated 2015-03-21 07:37:31 -0600

In the trivial case where the origin of /map and your grid 0,0 are the same and there is no rotation between the two, you need a pixel to real world units (meters) scale factor. OccupancyGrid has resolution in meters/cell. So pose.x / resolution = cell_x. In your example the x is negative so the cell coordinate is not in your map at all, you probably want the the center of the grid at 2000,2000 to be where the /map origin is, so pose.x / resolution + 2000 = cell_x.

You should look at using nav_msgs/OccupancyGrid which has the 2d array as well as the additional information ( ) required to transform from frame_id coordinates into pixel locations. looks like it would be useful but hasn't been getting maintained. Also look at .

I've uploaded your image because external links are unreliable over time (I assume you don't have the minimum points to upload images? Also github is giving me unicorn error messages right now so I can't see the robot publisher code...)

image description

edit flag offensive delete link more


ok I understood the linear conversion (x, y)...I have one other question: does the orientation between /map and /odom get into this? In which case, how do I cater for that?

RND gravatar image RND  ( 2015-03-21 10:27:23 -0600 )edit

The tf transform lookup takes care of that for you. If your grid is in map frame, then asking tf for the robot position in map frame gives you coordinates you don't have to do any subsequent rotations on.

lucasw gravatar image lucasw  ( 2015-03-23 11:00:28 -0600 )edit

Thank you for this, saved me a bunch of time. Quick question, I'm also trying to incorporate the orientation so I was wondering if you knew the best way to go about converting the quaternion to a simple yaw value in radians?

sr_corey gravatar image sr_corey  ( 2019-06-25 15:09:17 -0600 )edit

To go from a quaternion that represents a yaw angle to the plain yaw angle in radians you can use the function tf2::getYaw(robot_pose.orientation), where robot_pose is a geometry_msgs::Pose element.

jfrascon gravatar image jfrascon  ( 2020-04-14 01:41:01 -0600 )edit

thank you for your answer, saved my day.

Anas Kanhouch gravatar image Anas Kanhouch  ( 2022-02-03 01:08:17 -0600 )edit

Question Tools



Asked: 2015-03-21 04:40:27 -0600

Seen: 12,108 times

Last updated: Mar 21 '15