GMapping OccupancyGrid Coordinates
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.