# Robot coordinates in map

Hi,

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:

position:
x: -9.87982119798
y: 16.8167273361
z: 0.0
orientation:
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.

Originally posted by RND on ROS Answers with karma: 133 on 2015-03-21

Post score: 6

Comment by RND on 2015-05-04:
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

Comment by RND on 2015-05-04:
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 + (map.info.height - y -1)*map.info.width

Comment by sobot on 2015-05-04:
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

Comment by RND on 2015-05-04:
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!

Comment by sobot on 2015-05-04:
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.

Comment by RND on 2015-05-04:
so you are incorporating the path planning function with the frontier exploration algorithm all in one?

Comment by sobot on 2015-05-04:
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

Comment by RND on 2015-05-04:
I was going to use the move_base package for navigation. How does that sound?

Comment by sobot on 2015-05-04:
well, for frontier exploration scenarios like this you just need a simple trajectory follower to reach the chosen frontier cell (x,y) and then next n so on. Therefor a simple follower code would do the job, something simple and light. e.g. Carrot Planner type of navigation would do just fine. my idé

Comment by RND on 2015-05-05:
seems legit. by any chance, which mapping algorithm are you using? I'm using gmapping and i see that it is very susceptible to sharp rotations such that if the robot rotates on its own axis by 360 degrees, or if it turns and revisits certain areas, the resulting map is a mess. ideas?

Comment by sobot on 2015-05-05:
U can use gmapping or u can write ur own mapping node. But the problem that u r telling now seems to be coming from bad odometry rather than the mapping method. Does the same problem persist with gmapping and teleop too?

Comment by RND on 2015-05-05:
yes, while i manually steer the robot (by joystick) and use gmapping, I get the same problems. I checked the odometry data according to tutorials and it seems to be good. I have built maps but while moving the robot forward and closing loops (i.e. no sharp rotations or place revisiting).

Comment by sobot on 2015-05-05:
could be refresh rate (update rate), mismatch Odom. check your parameters. coz gmapping basically is based on localisation data and mapping. so when localisation is off, this problem happens. have u ever used an alrdy available Explorer package successfully? (i.e. for autonomous navigation of robot)

Comment by RND on 2015-05-05:
how can localization be "off" in gmapping? I'm setting map_update_interval to 0.1. No I havent used a ready exploration package successfully, i just started writing my own so that I could change the objective function the way i wanted it.

Comment by sobot on 2015-05-05:
by off i dont mean "turned off", i mean incorrect readings, inconsistent.

Comment by RND on 2015-05-05:
how can I know if that is the case?

Comment by sobot on 2015-05-05:
you can write a simple out and back node for your robot to perform; instruct it to go a X meters straight, stop and 360 rotate at the end of the line and come back X meters again. watch the process on RVIZ "Odom". see how it looks in real life and Odom in Rviz. u can compare them.

Comment by RND on 2015-05-06:
odometry seems to be fine according to my observations see this link. Can it be that im setting linearUpdate (=0.1) and angularUpdate (=0.05) too low?

Comment by RND on 2015-05-06:
Here you can see that performing the same test while mapping produces a disastrous result. what can be the issue?

Comment by sobot on 2015-05-06:
Try lowering your robot's linear speed (test different settings). those update parameters could have an effect, depending on ur equipment u may have to find the optimum setting. also check here.

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 ( http://docs.ros.org/indigo/api/nav_msgs/html/msg/MapMetaData.html ) required to transform from frame_id coordinates into pixel locations. http://wiki.ros.org/occupancy_grid_utils looks like it would be useful but hasn't been getting maintained. Also look at http://wiki.ros.org/costmap_2d.

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

Originally posted by lucasw with karma: 8729 on 2015-03-21

This answer was ACCEPTED on the original site

Post score: 8

Comment by RND on 2015-03-21:
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?

Comment by lucasw on 2015-03-23:
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.

Comment by sr_corey on 2019-06-25:
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?

Comment by jfrascon on 2020-04-14:
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.

Comment by Anas Kanhouch on 2022-02-03: