Program Listing for File costmap_2d_converter.hpp
↰ Return to documentation for file (include/grid_map_costmap_2d/costmap_2d_converter.hpp
)
/*
* costmap_2d_converter.hpp
*
* Created on: Nov 23, 2016
* Author: Peter Fankhauser, ETH Zurich
* Stefan Kohlbrecher, TU Darmstadt
* Daniel Stonier, Yujin Robot
* Gabriel Hottiger, ANYbotics
*/
#ifndef GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_
#define GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_
// STD
#include <cstdint>
#include <vector>
#include <string>
// ROS
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "grid_map_core/grid_map_core.hpp"
namespace grid_map
{
template<int64_t noInformation, int64_t lethalObstacle, int64_t inscribedInflatedObstacle,
int64_t freeSpace>
class Costmap2DTranslationTable
{
// Check required pre-conditions of template arguments
static_assert(
freeSpace < inscribedInflatedObstacle,
"[Costmap2DTranslationTable] Condition violated: freeSpace < inscribedInflatedObstacle.");
static_assert(
inscribedInflatedObstacle < lethalObstacle,
"[Costmap2DTranslationTable] Condition violated: inscribedInflatedObstacle < lethalObstacle.");
static_assert(
(noInformation < freeSpace) || (noInformation > lethalObstacle),
"[Costmap2DTranslationTable] Condition violated: "
"noInformation < freeSpace || noInformation > lethalObstacle.");
public:
// Only static methods -> delete constructor.
Costmap2DTranslationTable() = delete;
template<typename DataType>
static void create(std::vector<DataType> & costTranslationTable)
{
costTranslationTable.resize(256);
for (unsigned int i = 0; i < costTranslationTable.size(); ++i) {
costTranslationTable[i] = fromCostmap<DataType>(static_cast<uint8_t>(i));
}
}
template<typename DataType>
static DataType fromCostmap(const uint8_t costmapValue)
{
// Check special cost map values.
if (costmapValue == nav2_costmap_2d::FREE_SPACE) {
return freeSpace;
} else if (costmapValue == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
return inscribedInflatedObstacle;
} else if (costmapValue == nav2_costmap_2d::LETHAL_OBSTACLE) {
return lethalObstacle;
} else if (costmapValue == nav2_costmap_2d::NO_INFORMATION) {
return noInformation;
}
// Map costmap map interval to gridmap interval
// for values between free space and inflated obstacle
constexpr DataType costmapIntervalStart = nav2_costmap_2d::FREE_SPACE;
constexpr DataType
costmapIntervalWidth = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart;
constexpr DataType gridmapIntervalStart = freeSpace;
constexpr DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart;
const DataType interpolatedValue = gridmapIntervalStart + (
costmapValue - costmapIntervalStart) * gridmapIntervalWidth / costmapIntervalWidth;
return interpolatedValue;
}
template<typename DataType>
static uint8_t toCostmap(const DataType gridmapValue)
{
// Check special grid map values.
if (gridmapValue == static_cast<DataType>(noInformation)) {
return nav2_costmap_2d::NO_INFORMATION;
} else if (gridmapValue <= static_cast<DataType>(freeSpace)) {
return nav2_costmap_2d::FREE_SPACE;
} else if (gridmapValue >= static_cast<DataType>(lethalObstacle)) {
return nav2_costmap_2d::LETHAL_OBSTACLE;
} else if (gridmapValue >= static_cast<DataType>(inscribedInflatedObstacle)) {
return nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
}
// Map gridmap interval to costmap interval for values between free space and inflated obstacle
constexpr DataType costmapIntervalStart = nav2_costmap_2d::FREE_SPACE;
constexpr DataType
costmapIntervalWidth = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart;
constexpr DataType gridmapIntervalStart = freeSpace;
constexpr DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart;
const DataType interpolatedValue = costmapIntervalStart + (
gridmapValue - gridmapIntervalStart) * costmapIntervalWidth / gridmapIntervalWidth;
return std::round(interpolatedValue);
}
};
using Costmap2DDirectTranslationTable = Costmap2DTranslationTable<nav2_costmap_2d::NO_INFORMATION,
nav2_costmap_2d::LETHAL_OBSTACLE,
nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,
nav2_costmap_2d::FREE_SPACE>;
using Costmap2DCenturyTranslationTable =
Costmap2DTranslationTable<-1, 100, 99, nav2_costmap_2d::FREE_SPACE>;
template<typename DataType>
class Costmap2DDefaultTranslationTable
: public Costmap2DDirectTranslationTable
{
};
template<>
class Costmap2DDefaultTranslationTable<float>
: public Costmap2DCenturyTranslationTable
{
};
/*****************************************************************************
** Converter
*****************************************************************************/
template<typename MapType,
typename TranslationTable = Costmap2DDefaultTranslationTable<typename MapType::DataType>>
class Costmap2DConverter
{
public:
Costmap2DConverter() = default;
virtual ~Costmap2DConverter() = default;
void initializeFromGridMap(const MapType & gridMap, nav2_costmap_2d::Costmap2D & outputCostmap)
{
// Different origin position
const Position position = gridMap.getPosition() - Position(0.5 * gridMap.getLength());
const Size sizeXY = gridMap.getSize();
outputCostmap.resizeMap(
sizeXY(0),
sizeXY(1),
gridMap.getResolution(),
position(0),
position(1));
}
bool setCostmap2DFromGridMap(
const MapType & gridMap,
const std::string & layer,
nav2_costmap_2d::Costmap2D & outputCostmap)
{
// Check compliance.
Size size(outputCostmap.getSizeInCellsX(), outputCostmap.getSizeInCellsY());
if ((gridMap.getSize() != size).any()) {
errorMessage_ = "Costmap2D and output map have different sizes!";
return false;
}
if (!gridMap.getStartIndex().isZero()) {
errorMessage_ = "Does not support non-zero start indices!";
return false;
}
// Copy data.
// Reverse iteration is required because of different conventions
// between Costmap2D and grid map.
const size_t nCells = gridMap.getSize().prod();
for (size_t i = 0, j = nCells - 1; i < nCells; ++i, --j) {
outputCostmap.getCharMap()[j] =
TranslationTable::template toCostmap<DataType>(gridMap.get(layer).data()[i]);
}
return true;
}
void initializeFromCostmap2D(nav2_costmap_2d::Costmap2DROS & costmap2d, MapType & outputMap)
{
initializeFromCostmap2D(*(costmap2d.getCostmap()), outputMap);
outputMap.setFrameId(costmap2d.getGlobalFrameID());
}
void initializeFromCostmap2D(const nav2_costmap_2d::Costmap2D & costmap2d, MapType & outputMap)
{
const double resolution = costmap2d.getResolution();
Length
length(costmap2d.getSizeInCellsX() * resolution, costmap2d.getSizeInCellsY() * resolution);
Position position(costmap2d.getOriginX(), costmap2d.getOriginY());
// Different conventions.
position += Position(0.5 * length);
outputMap.setGeometry(length, resolution, position);
}
bool addLayerFromCostmap2D(
const nav2_costmap_2d::Costmap2D & costmap2d,
const std::string & layer,
MapType & outputMap)
{
// Check compliance.
Size size(costmap2d.getSizeInCellsX(), costmap2d.getSizeInCellsY());
if ((outputMap.getSize() != size).any()) {
errorMessage_ = "Costmap2D and output map have different sizes!";
return false;
}
if (!outputMap.getStartIndex().isZero()) {
errorMessage_ = "Does not support non-zero start indices!";
return false;
}
// Copy data.
// Reverse iteration is required because of different conventions
// between Costmap2D and grid map.
typename MapType::Matrix data(size(0), size(1));
const size_t nCells = outputMap.getSize().prod();
for (size_t i = 0, j = nCells - 1; i < nCells; ++i, --j) {
const unsigned char cost = costmap2d.getCharMap()[j];
data(i) = TranslationTable::template fromCostmap<DataType>(cost);
}
outputMap.add(layer, data);
return true;
}
bool addLayerFromCostmap2D(
nav2_costmap_2d::Costmap2DROS & costmap2d,
const std::string & layer,
MapType & outputMap)
{
return addLayerFromCostmap2D(*(costmap2d.getCostmap()), layer, outputMap);
}
bool initializeFromCostmap2DAtRobotPose(
nav2_costmap_2d::Costmap2DROS & costmap2d,
const Length & length,
MapType & outputMap)
{
const double resolution = costmap2d.getCostmap()->getResolution();
geometry_msgs::msg::PoseStamped tfPose;
if (!costmap2d.getRobotPose(tfPose)) {
errorMessage_ = "Could not get robot pose, is it actually published?";
return false;
}
Position robotPosition(tfPose.pose.position.x, tfPose.pose.position.y);
// Determine new costmap origin.
Position
rosMapOrigin(costmap2d.getCostmap()->getOriginX(), costmap2d.getCostmap()->getOriginY());
Position newCostMapOrigin;
// Note:
// You cannot directly use the robot pose as the new 'costmap center'
// since the underlying grid is not necessarily exactly aligned with
// that (two cases to consider, rolling window and globally fixed).
//
// Relevant diagrams:
// - https://github.com/anybotics/grid_map
// Float versions of the cell co-ordinates, use static_cast<int> to get the indices.
Position robotCellPosition = (robotPosition - rosMapOrigin) / resolution;
// if there is an odd number of cells
// centre of the new grid map in the centre of the current cell
// if there is an even number of cells
// centre of the new grid map at the closest vertex between cells
// of the current cell
int numberOfCellsX = static_cast<int>(length.x() / resolution);
int numberOfCellsY = static_cast<int>(length.y() / resolution);
if (numberOfCellsX % 2) { // odd
newCostMapOrigin(0) =
std::floor(robotCellPosition.x()) * resolution + resolution / 2.0 + rosMapOrigin.x();
} else {
newCostMapOrigin(0) = std::round(robotCellPosition.x()) * resolution + rosMapOrigin.x();
}
if (numberOfCellsY % 2) { // odd
newCostMapOrigin(1) =
std::floor(robotCellPosition.y()) * resolution + resolution / 2.0 + rosMapOrigin.y();
} else {
newCostMapOrigin(1) = std::round(robotCellPosition.y()) * resolution + rosMapOrigin.y();
}
// TODO(tbd): check the robot pose is in the window
// TODO(tbd): check the geometry fits within the costmap2d window
// Initialize the output map.
outputMap.setFrameId(costmap2d.getGlobalFrameID());
outputMap.setTimestamp(costmap2d.now().nanoseconds());
outputMap.setGeometry(length, resolution, newCostMapOrigin);
return true;
}
bool addLayerFromCostmap2DAtRobotPose(
nav2_costmap_2d::Costmap2DROS & costmap2d,
const std::string & layer,
MapType & outputMap)
{
/****************************************
** Asserts
****************************************/
if (outputMap.getResolution() != costmap2d.getCostmap()->getResolution()) {
errorMessage_ = "Costmap2D and output map have different resolutions!";
return false;
}
// 1) would be nice to check the output map centre has been initialised where it should be
// i.e. the robot pose didn't move since or the initializeFrom wasn't called
// but this would mean listening to tf's again and anyway, it gets shifted to make sure
// the costmaps align, so these wouldn't be exactly the same anyway
// 2) check the geometry fits inside the costmap2d subwindow is done below
// Properties.
const Length geometry = outputMap.getLength();
const Position position = outputMap.getPosition();
// Copy data.
bool isValidWindow;
nav2_costmap_2d::Costmap2D costmapSubwindow;
// TODO(tbd)
isValidWindow = costmapSubwindow.copyCostmapWindow(
*(costmap2d.getCostmap()),
position.x() - geometry.x() / 2.0, // subwindow_bottom_left_x
position.y() - geometry.y() / 2.0, // subwindow_bottom_left_y
geometry.x(), geometry.y());
if (!isValidWindow) {
// handle differently - e.g. copy the internal part only and lethal elsewhere,
// but other parts would have to handle being outside too
errorMessage_ = "Subwindow landed outside the costmap, aborting.";
return false;
}
addLayerFromCostmap2D(costmapSubwindow, layer, outputMap);
return true;
}
std::string errorMessage() const {return errorMessage_;}
private:
std::string errorMessage_;
};
} // namespace grid_map
#endif // GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_