| 35 | } |
| 36 | |
| 37 | void ImageToGridmapDemo::imageCallback(const sensor_msgs::Image& msg) |
| 38 | { |
| 39 | if (!mapInitialized_) { |
| 40 | grid_map::GridMapRosConverter::initializeFromImage(msg, resolution_, map_); |
| 41 | ROS_INFO("Initialized map with size %f x %f m (%i x %i cells).", map_.getLength().x(), |
| 42 | map_.getLength().y(), map_.getSize()(0), map_.getSize()(1)); |
| 43 | mapInitialized_ = true; |
| 44 | } |
| 45 | grid_map::GridMapRosConverter::addLayerFromImage(msg, "elevation", map_, minHeight_, maxHeight_); |
| 46 | grid_map::GridMapRosConverter::addColorLayerFromImage(msg, "color", map_); |
| 47 | |
| 48 | // Publish as grid map. |
| 49 | grid_map_msgs::GridMap mapMessage; |
| 50 | grid_map::GridMapRosConverter::toMessage(map_, mapMessage); |
| 51 | gridMapPublisher_.publish(mapMessage); |
| 52 | } |
| 53 | |
| 54 | } /* namespace */ |