Ask Your Question
3

Robot coordinates in map

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

RND gravatar image

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.

Thank you in advance.

edit retag flag offensive close merge delete

Comments

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 imageRND ( 2015-05-04 02:56:35 -0500 )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 + (map.info.height - y -1)*map.info.width

RND gravatar imageRND ( 2015-05-04 02:57:59 -0500 )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 imagesobot ( 2015-05-04 03:52:38 -0500 )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 imageRND ( 2015-05-04 04:16:09 -0500 )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 imagesobot ( 2015-05-04 05:56:20 -0500 )edit

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

RND gravatar imageRND ( 2015-05-04 06:24:02 -0500 )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 imagesobot ( 2015-05-04 07:36:33 -0500 )edit

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

RND gravatar imageRND ( 2015-05-04 07:55:19 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
4

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

lucasw gravatar image

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

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

image description

edit flag offensive delete link more

Comments

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 imageRND ( 2015-03-21 10:27:23 -0500 )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 imagelucasw ( 2015-03-23 11:00:28 -0500 )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 imagesr_corey ( 2019-06-25 15:09:17 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

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

Seen: 6,373 times

Last updated: Mar 21 '15