.. _program_listing_file__tmp_ws_src_grid_map_grid_map_costmap_2d_include_grid_map_costmap_2d_costmap_2d_converter.hpp:

Program Listing for File costmap_2d_converter.hpp
=================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_grid_map_grid_map_costmap_2d_include_grid_map_costmap_2d_costmap_2d_converter.hpp>` (``/tmp/ws/src/grid_map/grid_map_costmap_2d/include/grid_map_costmap_2d/costmap_2d_converter.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * 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_