Program Listing for File costmap_2d_converter.hpp

↰ Return to documentation for file (/tmp/ws/src/grid_map/grid_map_costmap_2d/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_