Costmap2DConverter.hpp
Go to the documentation of this file.
1 /*
2  * CostmapConverter.hpp
3  *
4  * Created on: Nov 23, 2016
5  * Author: Peter Fankhauser, ETH Zurich
6  * Stefan Kohlbrecher, TU Darmstadt
7  * Daniel Stonier, Yujin Robot
8  * Gabriel Hottiger, ANYbotics
9  */
10 
11 #pragma once
12 
14 
15 // ROS
16 #include <costmap_2d/costmap_2d.h>
18 #include <tf/tf.h>
19 
20 // STD
21 #include <stdint.h>
22 #include <vector>
23 
24 namespace grid_map {
25 
38 template <int64_t noInformation, int64_t lethalObstacle, int64_t inscribedInflatedObstacle, int64_t freeSpace>
40  // Check required pre-conditions of template arguments
41  static_assert(freeSpace < inscribedInflatedObstacle,
42  "[Costmap2DTranslationTable] Condition violated: freeSpace < inscribedInflatedObstacle.");
43  static_assert(inscribedInflatedObstacle < lethalObstacle,
44  "[Costmap2DTranslationTable] Condition violated: inscribedInflatedObstacle < lethalObstacle.");
45  static_assert(noInformation < freeSpace || noInformation > lethalObstacle,
46  "[Costmap2DTranslationTable] Condition violated: noInformation < freeSpace || noInformation > lethalObstacle.");
47 
48  public:
49  // Only static methods -> delete constructor.
50  Costmap2DTranslationTable() = delete;
51 
57  template <typename DataType>
58  static void create(std::vector<DataType>& costTranslationTable) {
59  costTranslationTable.resize(256);
60  for (unsigned int i = 0; i < costTranslationTable.size(); ++i) {
61  costTranslationTable[i] = fromCostmap<DataType>(static_cast<uint8_t>(i));
62  }
63  }
64 
71  template <typename DataType>
72  static DataType fromCostmap(const uint8_t costmapValue) {
73  // Check special cost map values.
74  if (costmapValue == costmap_2d::FREE_SPACE) {
75  return freeSpace;
76  } else if (costmapValue == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
77  return inscribedInflatedObstacle;
78  } else if (costmapValue == costmap_2d::LETHAL_OBSTACLE) {
79  return lethalObstacle;
80  } else if (costmapValue == costmap_2d::NO_INFORMATION) {
81  return noInformation;
82  }
83 
84  // Map costmap map interval to gridmap interval for values between free space and inflated obstacle
85  constexpr DataType costmapIntervalStart = costmap_2d::FREE_SPACE;
86  constexpr DataType costmapIntervalWidth = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart;
87  constexpr DataType gridmapIntervalStart = freeSpace;
88  constexpr DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart;
89  const DataType interpolatedValue =
90  gridmapIntervalStart + (costmapValue - costmapIntervalStart) * gridmapIntervalWidth / costmapIntervalWidth;
91  return interpolatedValue;
92  }
93 
100  template <typename DataType>
101  static uint8_t toCostmap(const DataType gridmapValue) {
102  // Check special grid map values.
103  if (gridmapValue == static_cast<DataType>(noInformation)) {
105  } else if (gridmapValue <= static_cast<DataType>(freeSpace)) {
106  return costmap_2d::FREE_SPACE;
107  } else if (gridmapValue >= static_cast<DataType>(lethalObstacle)) {
109  } else if (gridmapValue >= static_cast<DataType>(inscribedInflatedObstacle)) {
111  }
112 
113  // Map gridmap interval to costmap interval for values between free space and inflated obstacle
114  constexpr DataType costmapIntervalStart = costmap_2d::FREE_SPACE;
115  constexpr DataType costmapIntervalWidth = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart;
116  constexpr DataType gridmapIntervalStart = freeSpace;
117  constexpr DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart;
118  const DataType interpolatedValue =
119  costmapIntervalStart + (gridmapValue - gridmapIntervalStart) * costmapIntervalWidth / gridmapIntervalWidth;
120 
121  return std::round(interpolatedValue);
122  }
123 };
124 
131 
136 using Costmap2DCenturyTranslationTable = Costmap2DTranslationTable<-1, 100, 99, costmap_2d::FREE_SPACE>;
137 
138 template <typename DataType>
140 
141 template <>
143 
144 /*****************************************************************************
145 ** Converter
146 *****************************************************************************/
147 
156 template <typename MapType, typename TranslationTable = Costmap2DDefaultTranslationTable<typename MapType::DataType>>
158  public:
159  Costmap2DConverter() = default;
160  virtual ~Costmap2DConverter() = default;
161 
167  void initializeFromGridMap(const MapType& gridMap, costmap_2d::Costmap2D& outputCostmap) {
168  // Different origin position
169  const Position position = gridMap.getPosition() - Position(0.5 * gridMap.getLength());
170  const Size sizeXY = gridMap.getSize();
171  outputCostmap.resizeMap(sizeXY(0), sizeXY(1), gridMap.getResolution(), position(0), position(1));
172  }
173 
181  bool setCostmap2DFromGridMap(const MapType& gridMap, const std::string& layer, costmap_2d::Costmap2D& outputCostmap) {
182  // Check compliance.
183  Size size(outputCostmap.getSizeInCellsX(), outputCostmap.getSizeInCellsY());
184  if ((gridMap.getSize() != size).any()) {
185  errorMessage_ = "Costmap2D and output map have different sizes!";
186  return false;
187  }
188  if (!gridMap.getStartIndex().isZero()) {
189  errorMessage_ = "Does not support non-zero start indices!";
190  return false;
191  }
192  // Copy data.
193  // Reverse iteration is required because of different conventions
194  // between Costmap2D and grid map.
195  const size_t nCells = gridMap.getSize().prod();
196  for (size_t i = 0, j = nCells - 1; i < nCells; ++i, --j) {
197  outputCostmap.getCharMap()[j] = TranslationTable::template toCostmap<DataType>(gridMap.get(layer).data()[i]);
198  }
199  return true;
200  }
201 
202  void initializeFromCostmap2D(costmap_2d::Costmap2DROS& costmap2d, MapType& outputMap) {
203  initializeFromCostmap2D(*(costmap2d.getCostmap()), outputMap);
204  outputMap.setFrameId(costmap2d.getGlobalFrameID());
205  }
206 
207  void initializeFromCostmap2D(const costmap_2d::Costmap2D& costmap2d, MapType& outputMap) {
208  const double resolution = costmap2d.getResolution();
209  Length length(costmap2d.getSizeInCellsX() * resolution, costmap2d.getSizeInCellsY() * resolution);
210  Position position(costmap2d.getOriginX(), costmap2d.getOriginY());
211  // Different conventions.
212  position += Position(0.5 * length);
213  outputMap.setGeometry(length, resolution, position);
214  }
215 
224  bool addLayerFromCostmap2D(const costmap_2d::Costmap2D& costmap2d, const std::string& layer, MapType& outputMap) {
225  // Check compliance.
226  Size size(costmap2d.getSizeInCellsX(), costmap2d.getSizeInCellsY());
227  if ((outputMap.getSize() != size).any()) {
228  errorMessage_ = "Costmap2D and output map have different sizes!";
229  return false;
230  }
231  if (!outputMap.getStartIndex().isZero()) {
232  errorMessage_ = "Does not support non-zero start indices!";
233  return false;
234  }
235  // Copy data.
236  // Reverse iteration is required because of different conventions
237  // between Costmap2D and grid map.
238  typename MapType::Matrix data(size(0), size(1));
239  const size_t nCells = outputMap.getSize().prod();
240  for (size_t i = 0, j = nCells - 1; i < nCells; ++i, --j) {
241  const unsigned char cost = costmap2d.getCharMap()[j];
242  data(i) = TranslationTable::template fromCostmap<DataType>(cost);
243  }
244 
245  outputMap.add(layer, data);
246  return true;
247  }
248 
257  bool addLayerFromCostmap2D(costmap_2d::Costmap2DROS& costmap2d, const std::string& layer, MapType& outputMap) {
258  return addLayerFromCostmap2D(*(costmap2d.getCostmap()), layer, outputMap);
259  }
260 
276  bool initializeFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS& costmap2d, const Length& length, MapType& outputMap) {
277  const double resolution = costmap2d.getCostmap()->getResolution();
278 
279  // Get the Robot Pose Transform.
280 #if ROS_VERSION_MINIMUM(1, 14, 0)
281  geometry_msgs::PoseStamped tfPose;
282  if (!costmap2d.getRobotPose(tfPose)) {
283  errorMessage_ = "Could not get robot pose, is it actually published?";
284  return false;
285  }
286  Position robotPosition(tfPose.pose.position.x, tfPose.pose.position.y);
287 #else
288  tf::Stamped<tf::Pose> tfPose;
289  if (!costmap2d.getRobotPose(tfPose)) {
290  errorMessage_ = "Could not get robot pose, is it actually published?";
291  return false;
292  }
293  Position robotPosition(tfPose.getOrigin().x(), tfPose.getOrigin().y());
294 #endif
295  // Determine new costmap origin.
296  Position rosMapOrigin(costmap2d.getCostmap()->getOriginX(), costmap2d.getCostmap()->getOriginY());
297  Position newCostMapOrigin;
298 
299  // Note:
300  // You cannot directly use the robot pose as the new 'costmap center'
301  // since the underlying grid is not necessarily exactly aligned with
302  // that (two cases to consider, rolling window and globally fixed).
303  //
304  // Relevant diagrams:
305  // - https://github.com/anybotics/grid_map
306 
307  // Float versions of the cell co-ordinates, use static_cast<int> to get the indices.
308  Position robotCellPosition = (robotPosition - rosMapOrigin) / resolution;
309 
310  // if there is an odd number of cells
311  // centre of the new grid map in the centre of the current cell
312  // if there is an even number of cells
313  // centre of the new grid map at the closest vertex between cells
314  // of the current cell
315  int numberOfCellsX = length.x() / resolution;
316  int numberOfCellsY = length.y() / resolution;
317  if (numberOfCellsX % 2) { // odd
318  newCostMapOrigin(0) = std::floor(robotCellPosition.x()) * resolution + resolution / 2.0 + rosMapOrigin.x();
319  } else {
320  newCostMapOrigin(0) = std::round(robotCellPosition.x()) * resolution + rosMapOrigin.x();
321  }
322  if (numberOfCellsY % 2) { // odd
323  newCostMapOrigin(1) = std::floor(robotCellPosition.y()) * resolution + resolution / 2.0 + rosMapOrigin.y();
324  } else {
325  newCostMapOrigin(1) = std::round(robotCellPosition.y()) * resolution + rosMapOrigin.y();
326  }
327 
328  // TODO check the robot pose is in the window
329  // TODO check the geometry fits within the costmap2d window
330 
331  // Initialize the output map.
332  outputMap.setFrameId(costmap2d.getGlobalFrameID());
333  outputMap.setTimestamp(ros::Time::now().toNSec());
334  outputMap.setGeometry(length, resolution, newCostMapOrigin);
335  return true;
336  }
337 
347  bool addLayerFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS& costmap2d, const std::string& layer, MapType& outputMap) {
348  /****************************************
349  ** Asserts
350  ****************************************/
351  if (outputMap.getResolution() != costmap2d.getCostmap()->getResolution()) {
352  errorMessage_ = "Costmap2D and output map have different resolutions!";
353  return false;
354  }
355  // 1) would be nice to check the output map centre has been initialised where it should be
356  // i.e. the robot pose didn't move since or the initializeFrom wasn't called
357  // but this would mean listening to tf's again and anyway, it gets shifted to make sure
358  // the costmaps align, so these wouldn't be exactly the same anyway
359  // 2) check the geometry fits inside the costmap2d subwindow is done below
360 
361  // Properties.
362  const double resolution = costmap2d.getCostmap()->getResolution();
363  const Length geometry = outputMap.getLength();
364  const Position position = outputMap.getPosition();
365 
366  // Copy data.
367  bool isValidWindow = false;
368  costmap_2d::Costmap2D costmapSubwindow;
369  // TODO
370  isValidWindow = costmapSubwindow.copyCostmapWindow(*(costmap2d.getCostmap()),
371  position.x() - geometry.x() / 2.0, // subwindow_bottom_left_x
372  position.y() - geometry.y() / 2.0, // subwindow_bottom_left_y
373  geometry.x(), geometry.y());
374  if (!isValidWindow) {
375  // handle differently - e.g. copy the internal part only and lethal elsewhere, but other parts would have to handle being outside too
376  errorMessage_ = "Subwindow landed outside the costmap, aborting.";
377  return false;
378  }
379  addLayerFromCostmap2D(costmapSubwindow, layer, outputMap);
380  return true;
381  }
382 
391  std::string errorMessage() const { return errorMessage_; }
392 
393  private:
394  std::string errorMessage_;
395 };
396 
397 } // namespace grid_map
Convert Costmap2DRos objects into cost or grid maps.
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
bool setCostmap2DFromGridMap(const MapType &gridMap, const std::string &layer, costmap_2d::Costmap2D &outputCostmap)
bool getRobotPose(tf::Stamped< tf::Pose > &global_pose) const
std::string errorMessage() const
Human readable error message string.
void initializeFromCostmap2D(costmap_2d::Costmap2DROS &costmap2d, MapType &outputMap)
Eigen::Array2i Size
std::string getGlobalFrameID()
double getOriginX() const
bool addLayerFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS &costmap2d, const std::string &layer, MapType &outputMap)
static const unsigned char FREE_SPACE
unsigned char * getCharMap() const
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Eigen::Vector2d Position
bool addLayerFromCostmap2D(costmap_2d::Costmap2DROS &costmap2d, const std::string &layer, MapType &outputMap)
void initializeFromGridMap(const MapType &gridMap, costmap_2d::Costmap2D &outputCostmap)
bool initializeFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS &costmap2d, const Length &length, MapType &outputMap)
double getOriginY() const
unsigned int getSizeInCellsY() const
Matrix::Scalar DataType
static void create(std::vector< DataType > &costTranslationTable)
bool copyCostmapWindow(const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y)
bool addLayerFromCostmap2D(const costmap_2d::Costmap2D &costmap2d, const std::string &layer, MapType &outputMap)
unsigned int getSizeInCellsX() const
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
static Time now()
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
double getResolution() const
Costmap2D * getCostmap()
void initializeFromCostmap2D(const costmap_2d::Costmap2D &costmap2d, MapType &outputMap)
static uint8_t toCostmap(const DataType gridmapValue)
static DataType fromCostmap(const uint8_t costmapValue)
Eigen::Array2d Length


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