Ask Your Question
0

GMapping OccupancyGrid Coordinates

asked 2015-06-14 07:37:26 -0600

ROSNoob gravatar image

Hi.

I'm having a little trouble representing the correct data from an OccupancyGrid created by GMapping.

My setup is a single robot in a STDR simulator with one laser.

I'm trying to read the pixel value from the OccupancyGrid for the current position of my robot. The relevant parts of my code are as follows:

 ros::ServiceClient client  = n.serviceClient<nav_msgs::GetMap>("/dynamic_map");

  const nav_msgs::OccupancyGrid& map (srv.response.map);

  try{
  listener.lookupTransform("/map", "/robot1",  
                             ros::Time(0), transform);
  }
  catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
  }

  grid_x = (transform.getOrigin().x() / map.info.resolution);
  grid_y = (transform.getOrigin().y() / map.info.resolution);

  currentPoint = map.data[grid_x * grid_y];

  ROS_INFO("Grid X: %d Grid Y: %d Value here: %d", grid_x, grid_y, currentPoint);

My problem is that currentPoint is always -1, however I can see that the map is being created using RViz, and if I edit the code to loop through the map each time there are plenty of 0 and 100 values.

I have a feeling my grid_x and grid_y might be being calculated wrong, but I'm not sure how. The origin of my map is 0,0,0, so I don't think adding that into the calculation would change anything right now.

Any help would be appreciated.

Thanks.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2015-06-15 11:52:19 -0600

ROSNoob gravatar image

updated 2015-06-16 08:01:22 -0600

Updating the code to:

grid_x = (transform.getOrigin().x() - (int)map.info.origin.position.x) / map.info.resolution;
grid_y = (transform.getOrigin().y() - (int)map.info.origin.position.y) / map.info.resolution;

And as per yigit's suggestion

currentPoint = map.data[grid_y * map.info.width + grid_x];

Fixed the problem. Turns out my origins weren't 0,0,0 like I thought.

Thanks.

edit flag offensive delete link more
1

answered 2015-06-14 11:49:17 -0600

yigit gravatar image

updated 2015-06-14 11:55:04 -0600

Here is the error:

currentPoint = map.data[grid_x * grid_y];

I believe currentPoint should be calculated as follows:

currentPoint = map.data[grid_x * mapWidth + grid_y];

or

currentPoint = map.data[grid_y * mapWidth + grid_x];

depending on your orientation. By the way, you can get mapWidth as follows:

mapWidth = map.info.width;
edit flag offensive delete link more

Comments

Hi.

Thanks for your reply.

I edited my code to your suggestion but it still didn't work, however your suggestion was correct. Please see my edited answer.

ROSNoob gravatar imageROSNoob ( 2015-06-15 11:51:14 -0600 )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

2 followers

Stats

Asked: 2015-06-14 07:37:26 -0600

Seen: 1,569 times

Last updated: Jun 16 '15