12 #include <grid_map_msgs/GridMap.h> 16 #include <unordered_map> 23 #include <sensor_msgs/PointCloud2.h> 24 #include <sensor_msgs/Image.h> 26 #include <nav_msgs/OccupancyGrid.h> 27 #include <nav_msgs/GridCells.h> 57 static bool fromMessage(
const grid_map_msgs::GridMap& message,
grid_map::GridMap& gridMap,
const std::vector<std::string>& layers,
bool copyBasicLayers =
true,
bool copyAllNonBasicLayers =
true);
81 grid_map_msgs::GridMap& message);
92 const std::string& pointLayer,
93 sensor_msgs::PointCloud2& pointCloud);
105 const std::vector<std::string>& layers,
106 const std::string& pointLayer,
107 sensor_msgs::PointCloud2& pointCloud);
129 float dataMin,
float dataMax, nav_msgs::OccupancyGrid& occupancyGrid);
142 float lowerThreshold,
float upperThreshold,
143 nav_msgs::GridCells& gridCells);
170 static bool addLayerFromImage(
const sensor_msgs::Image& image,
const std::string& layer,
172 const float upperValue = 1.0,
const double alphaThreshold = 0.5);
195 const std::string encoding, sensor_msgs::Image& image);
208 const std::string encoding,
const float lowerValue,
const float upperValue,
209 sensor_msgs::Image& image);
235 const std::string encoding,
const float lowerValue,
249 const std::string& topic);
258 static bool loadFromBag(
const std::string& pathToBag,
const std::string& topic,
static void toMessage(const grid_map::GridMap &gridMap, grid_map_msgs::GridMap &message)
static bool toImage(const grid_map::GridMap &gridMap, const std::string &layer, const std::string encoding, sensor_msgs::Image &image)
static bool fromOccupancyGrid(const nav_msgs::OccupancyGrid &occupancyGrid, const std::string &layer, grid_map::GridMap &gridMap)
static void toPointCloud(const grid_map::GridMap &gridMap, const std::string &pointLayer, sensor_msgs::PointCloud2 &pointCloud)
static void toOccupancyGrid(const grid_map::GridMap &gridMap, const std::string &layer, float dataMin, float dataMax, nav_msgs::OccupancyGrid &occupancyGrid)
static bool loadFromBag(const std::string &pathToBag, const std::string &topic, grid_map::GridMap &gridMap)
static bool initializeFromImage(const sensor_msgs::Image &image, const double resolution, grid_map::GridMap &gridMap, const grid_map::Position &position=grid_map::Position::Zero())
virtual ~GridMapRosConverter()
static bool fromMessage(const grid_map_msgs::GridMap &message, grid_map::GridMap &gridMap, const std::vector< std::string > &layers, bool copyBasicLayers=true, bool copyAllNonBasicLayers=true)
static bool saveToBag(const grid_map::GridMap &gridMap, const std::string &pathToBag, const std::string &topic)
static bool addColorLayerFromImage(const sensor_msgs::Image &image, const std::string &layer, grid_map::GridMap &gridMap)
static void toGridCells(const grid_map::GridMap &gridMap, const std::string &layer, float lowerThreshold, float upperThreshold, nav_msgs::GridCells &gridCells)
static bool toCvImage(const grid_map::GridMap &gridMap, const std::string &layer, const std::string encoding, cv_bridge::CvImage &cvImage)
static bool addLayerFromImage(const sensor_msgs::Image &image, const std::string &layer, grid_map::GridMap &gridMap, const float lowerValue=0.0, const float upperValue=1.0, const double alphaThreshold=0.5)