38 template <
int64_t noInformation,
int64_t lethalObstacle,
int64_t inscribedInflatedObstacle,
int64_t freeSpace>
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.");
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));
71 template <
typename DataType>
77 return inscribedInflatedObstacle;
79 return lethalObstacle;
87 constexpr
DataType gridmapIntervalStart = freeSpace;
88 constexpr
DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart;
90 gridmapIntervalStart + (costmapValue - costmapIntervalStart) * gridmapIntervalWidth / costmapIntervalWidth;
91 return interpolatedValue;
100 template <
typename DataType>
103 if (gridmapValue == static_cast<DataType>(noInformation)) {
105 }
else if (gridmapValue <= static_cast<DataType>(freeSpace)) {
107 }
else if (gridmapValue >= static_cast<DataType>(lethalObstacle)) {
109 }
else if (gridmapValue >= static_cast<DataType>(inscribedInflatedObstacle)) {
116 constexpr
DataType gridmapIntervalStart = freeSpace;
117 constexpr
DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart;
119 costmapIntervalStart + (gridmapValue - gridmapIntervalStart) * costmapIntervalWidth / gridmapIntervalWidth;
121 return std::round(interpolatedValue);
138 template <
typename DataType>
156 template <
typename MapType,
typename TranslationTable = Costmap2DDefaultTranslationTable<
typename MapType::DataType>>
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));
184 if ((gridMap.getSize() != size).any()) {
185 errorMessage_ =
"Costmap2D and output map have different sizes!";
188 if (!gridMap.getStartIndex().isZero()) {
189 errorMessage_ =
"Does not support non-zero start indices!";
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]);
203 initializeFromCostmap2D(*(costmap2d.
getCostmap()), outputMap);
213 outputMap.setGeometry(
length, resolution, position);
227 if ((outputMap.getSize() != size).any()) {
228 errorMessage_ =
"Costmap2D and output map have different sizes!";
231 if (!outputMap.getStartIndex().isZero()) {
232 errorMessage_ =
"Does not support non-zero start indices!";
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);
245 outputMap.add(layer, data);
258 return addLayerFromCostmap2D(*(costmap2d.
getCostmap()), layer, outputMap);
280 #if ROS_VERSION_MINIMUM(1, 14, 0) 281 geometry_msgs::PoseStamped tfPose;
283 errorMessage_ =
"Could not get robot pose, is it actually published?";
286 Position robotPosition(tfPose.pose.position.x, tfPose.pose.position.y);
290 errorMessage_ =
"Could not get robot pose, is it actually published?";
293 Position robotPosition(tfPose.getOrigin().x(), tfPose.getOrigin().y());
308 Position robotCellPosition = (robotPosition - rosMapOrigin) / resolution;
315 int numberOfCellsX = length.x() / resolution;
316 int numberOfCellsY = length.y() / resolution;
317 if (numberOfCellsX % 2) {
318 newCostMapOrigin(0) = std::floor(robotCellPosition.x()) * resolution + resolution / 2.0 + rosMapOrigin.x();
320 newCostMapOrigin(0) = std::round(robotCellPosition.x()) * resolution + rosMapOrigin.x();
322 if (numberOfCellsY % 2) {
323 newCostMapOrigin(1) = std::floor(robotCellPosition.y()) * resolution + resolution / 2.0 + rosMapOrigin.y();
325 newCostMapOrigin(1) = std::round(robotCellPosition.y()) * resolution + rosMapOrigin.y();
334 outputMap.setGeometry(length, resolution, newCostMapOrigin);
352 errorMessage_ =
"Costmap2D and output map have different resolutions!";
363 const Length geometry = outputMap.getLength();
364 const Position position = outputMap.getPosition();
367 bool isValidWindow =
false;
371 position.x() - geometry.x() / 2.0,
372 position.y() - geometry.y() / 2.0,
373 geometry.x(), geometry.y());
374 if (!isValidWindow) {
376 errorMessage_ =
"Subwindow landed outside the costmap, aborting.";
379 addLayerFromCostmap2D(costmapSubwindow, layer, outputMap);
Costmap2DTranslationTable()=delete
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)
std::string errorMessage_
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)
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
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
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
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
double getResolution() const
void initializeFromCostmap2D(const costmap_2d::Costmap2D &costmap2d, MapType &outputMap)
static uint8_t toCostmap(const DataType gridmapValue)
static DataType fromCostmap(const uint8_t costmapValue)