0
$\begingroup$

Rosanswers logo

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.


Originally posted by ROSNoob on ROS Answers with karma: 1 on 2015-06-14

Post score: 0

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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;

Originally posted by yigit with karma: 796 on 2015-06-14

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by ROSNoob on 2015-06-15:
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.

$\endgroup$
0
$\begingroup$

Rosanswers logo

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.


Originally posted by ROSNoob with karma: 1 on 2015-06-15

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.