origin conflict when converting from grid map to occupancy grid about grid_map HOT 9 CLOSED

@stonier FYI

from grid_map.

First issue, I'm guessing its due to the data layout from the top left corner (graphics style). I haven't done a costmap <-> cost_map (aka grid_map) conversion yet, but will shortly so I'll probably experience the same thing.

Second issue, just conventions I think. Pinning the grid_map in the centre or bottom left corner is neither right, nor wrong, just different ways of doing it. So long as all is clearly defined, I'm ok with that. They have included a useful diagram referencing the bits and pieces of the grid_map layout here.

from grid_map.

Regarding the second issue, I think it's fine, if grid map uses it's own convention. Though when converting to occupancy grids, the output should align with the convention (mainly) used within ROS.

from grid_map.

I didn't seem to have any problems loading an image into a cost_map (aka grid_map) and then to occupancy grid map - setting the frame id to "map" and cost map position to zero resulted in an occupancy grid oriented in the expected x-direction in rviz. It is using the same code. Picture here.

Is your conversion from an occupancy grid into a grid_map doing things correctly?

from grid_map.

I'm not changing any position or frame id, but use the source. Please post the pose/origin after your conversion from image (sensor_msgs::Image?) to cost_map/grid_map as well as the resulting occupancy grid message.

Here is my implementation of the conversion nav_msgs::OccupancyGrid -> grid_map::GridMap:

bool ElevatorController::occupancyGridToGridMap(const nav_msgs::OccupancyGrid& occupancy_grid_msg,
                                                grid_map::GridMap& grid_map)
{
  grid_map_msgs::GridMap grid_map_msg;
  grid_map_msg.info.header = occupancy_grid_msg.header;
  grid_map_msg.info.resolution = occupancy_grid_msg.info.resolution;
  grid_map_msg.info.length_x = occupancy_grid_msg.info.width * occupancy_grid_msg.info.resolution;
  grid_map_msg.info.length_y = occupancy_grid_msg.info.height * occupancy_grid_msg.info.resolution;
  grid_map_msg.info.pose = occupancy_grid_msg.info.origin;
  grid_map_msg.layers.resize(1);
  grid_map_msg.layers[0] = "occupancy_probabilities";
  grid_map_msg.basic_layers.resize(0);
  grid_map_msg.data.resize(1);
  grid_map_msg.data[0].layout.dim.resize(2);
  grid_map_msg.data[0].layout.dim[0].label = "column_index";
  grid_map_msg.data[0].layout.dim[0].size = occupancy_grid_msg.info.height;
  grid_map_msg.data[0].layout.dim[0].stride = occupancy_grid_msg.info.height * occupancy_grid_msg.info.width;
  grid_map_msg.data[0].layout.dim[1].label = "row_index";
  grid_map_msg.data[0].layout.dim[1].size = occupancy_grid_msg.info.width;
  grid_map_msg.data[0].layout.dim[1].stride = occupancy_grid_msg.info.width;
  grid_map_msg.data[0].layout.data_offset = 0;
  grid_map_msg.data[0].data.resize(occupancy_grid_msg.data.size());
  for (int cell = 0; cell < occupancy_grid_msg.data.size(); ++cell)
  {
    grid_map_msg.data[0].data[cell] = occupancy_grid_msg.data[cell];
  }
  grid_map_msg.outer_start_index = 0;
  grid_map_msg.inner_start_index = 0;

  if (grid_map::GridMapRosConverter::fromMessage(grid_map_msg, grid_map))
  {
#ifdef DEBUG
      pub_gridmap_.publish(grid_map_msg);
      nav_msgs::OccupancyGrid occ_grid_msg;
      grid_map::GridMapRosConverter::toOccupancyGrid(grid_map, grid_map.getLayers()[0], 0, 100, occ_grid_msg);
      pub_costmap_.publish(occ_grid_msg);
    }
#endif
    return true;
  }
  else
  {
    ROS_WARN_STREAM_THROTTLE(1.0,  "Couldn't convert grid map message into grid map.");
    return false;
  }
};
``

from grid_map.

@bit-pirate You can not just copy the origin from the OccupancyGrid to the GridMap, you have to convert it first. In the OccupancyGrid msg the origin represents the real-world pose of the cell (0, 0), while GridMap's position is w.r.t. the map center, as you can see at the layout that @stonier mentioned.
See here: http://docs.ros.org/api/nav_msgs/html/msg/MapMetaData.html

from grid_map.

Thanks a lot, @bit-pirate, @stonier, and @czalidis. I just added the conversion from occupancy map to grid map in the branch feature/occupancy_to_grid_map. Could you please test this for your use case? If all works out, I will merged this branch into master. If you have any unexpected behavior, please document it with a unit test which you can create in GridMapTest.cpp.

from grid_map.

This issue should is resolved with #36.

from grid_map.

Was fighting my own installation on Friday and finally got around testing your changes just now.

Works well for me. 👍 Thanks for this addition!

from grid_map.