GridMapRosConverter.hpp
Go to the documentation of this file.
1 /*
2  * GridMapRosConverter.hpp
3  *
4  * Created on: Jul 14, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
9 #pragma once
10 
12 #include <grid_map_msgs/GridMap.h>
13 
14 // STL
15 #include <vector>
16 #include <unordered_map>
17 
18 // Eigen
19 #include <Eigen/Core>
20 
21 // ROS
22 #include <ros/ros.h>
23 #include <sensor_msgs/PointCloud2.h>
24 #include <sensor_msgs/Image.h>
26 #include <nav_msgs/OccupancyGrid.h>
27 #include <nav_msgs/GridCells.h>
28 #include <cv_bridge/cv_bridge.h>
29 
30 namespace grid_map {
31 
36 {
37  public:
42 
46  virtual ~GridMapRosConverter();
47 
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);
58 
65  static bool fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap);
66 
72  static void toMessage(const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message);
73 
80  static void toMessage(const grid_map::GridMap& gridMap, const std::vector<std::string>& layers,
81  grid_map_msgs::GridMap& message);
82 
91  static void toPointCloud(const grid_map::GridMap& gridMap,
92  const std::string& pointLayer,
93  sensor_msgs::PointCloud2& pointCloud);
94 
104  static void toPointCloud(const grid_map::GridMap& gridMap,
105  const std::vector<std::string>& layers,
106  const std::string& pointLayer,
107  sensor_msgs::PointCloud2& pointCloud);
108 
116  static bool fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid,
117  const std::string& layer, grid_map::GridMap& gridMap);
118 
128  static void toOccupancyGrid(const grid_map::GridMap& gridMap, const std::string& layer,
129  float dataMin, float dataMax, nav_msgs::OccupancyGrid& occupancyGrid);
130 
141  static void toGridCells(const grid_map::GridMap& gridMap, const std::string& layer,
142  float lowerThreshold, float upperThreshold,
143  nav_msgs::GridCells& gridCells);
144 
154  static bool initializeFromImage(const sensor_msgs::Image& image, const double resolution,
155  grid_map::GridMap& gridMap,
156  const grid_map::Position& position = grid_map::Position::Zero());
157 
170  static bool addLayerFromImage(const sensor_msgs::Image& image, const std::string& layer,
171  grid_map::GridMap& gridMap, const float lowerValue = 0.0,
172  const float upperValue = 1.0, const double alphaThreshold = 0.5);
173 
181  static bool addColorLayerFromImage(const sensor_msgs::Image& image, const std::string& layer,
182  grid_map::GridMap& gridMap);
183 
194  static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
195  const std::string encoding, sensor_msgs::Image& image);
196 
207  static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
208  const std::string encoding, const float lowerValue, const float upperValue,
209  sensor_msgs::Image& image);
210 
221  static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
222  const std::string encoding, cv_bridge::CvImage& cvImage);
223 
234  static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
235  const std::string encoding, const float lowerValue,
236  const float upperValue, cv_bridge::CvImage& cvImage);
237 
248  static bool saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag,
249  const std::string& topic);
250 
258  static bool loadFromBag(const std::string& pathToBag, const std::string& topic,
259  grid_map::GridMap& gridMap);
260 
261 };
262 
263 } /* namespace */
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())
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)
Eigen::Vector2d Position
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)


grid_map_ros
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:35