Provide costmap to homotopy class planner

source/armarx/navigation/algorithms/Costmap.h

@@ -68,8 +68,10 @@ namespace armarx::navigation::algorithms
         centerPose() const
         {
             const Eigen::Vector2f costmap_P_center{
-                (getLocalSceneBounds().max.x() - getLocalSceneBounds().min.x())/2 + getLocalSceneBounds().min.x(),
-                (getLocalSceneBounds().max.y() - getLocalSceneBounds().min.y())/2 + getLocalSceneBounds().min.y()};
+                (getLocalSceneBounds().max.x() - getLocalSceneBounds().min.x()) / 2 +
+                    getLocalSceneBounds().min.x(),
+                (getLocalSceneBounds().max.y() - getLocalSceneBounds().min.y()) / 2 +
+                    getLocalSceneBounds().min.y()};
 
             return global_T_costmap * Eigen::Translation2f(costmap_P_center);
         }

source/armarx/navigation/local_planning/CMakeLists.txt

@@ -10,6 +10,7 @@ armarx_add_library(local_planning
         ArViz
         armarx_navigation::core
         armarx_navigation::conversions
+        armarx_navigation::algorithms
         armarx_navigation::local_planning_aron
         armarx_navigation::teb_human
         teb_extension::obstacles

source/armarx/navigation/local_planning/TimedElasticBands.cpp

@@ -67,8 +67,12 @@ namespace armarx::navigation::local_planning
 
         hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
         hcp_->initialize(
-            cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);
-
+            cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);       
+        //set member teb_costmap
+        setTebCostmap();
+        if (teb_costmap) {
+            hcp_->setCostmap(&teb_costmap.value());
+        }
         ros::Time::init(); // we have to init time before we can use the planner
     }
 
@@ -198,4 +202,17 @@ namespace armarx::navigation::local_planning
         ARMARX_INFO << "TEB: added " << obstManager.size() << " obstacles";
     }
 
+    //export algorithms::Costmap to costmap type of teb local planner and provide costmap to planner
+    void
+    TimedElasticBands::setTebCostmap()
+    {
+        if (!scene.staticScene)
+            return;
+        if (!scene.staticScene->distanceToObstaclesCostmap)
+            return;
+        const algorithms::Costmap& navigationCostmap =
+            scene.staticScene->distanceToObstaclesCostmap.value();
+        teb_costmap.emplace(conv::toRos(navigationCostmap));
+    }
+
 } // namespace armarx::navigation::local_planning

source/armarx/navigation/local_planning/TimedElasticBands.h

@@ -61,6 +61,7 @@ namespace armarx::navigation::local_planning
     private:
         void readDefaultConfig(arondto::TimedElasticBandsParams& target);
         void fillObstacles();
+        void setTebCostmap();
 
     protected:
         Params params;
@@ -72,6 +73,7 @@ namespace armarx::navigation::local_planning
         teb_local_planner::ObstContainer teb_obstacles;
         TebObstacleManager obstManager;
         teb_local_planner::PoseSequence teb_globalPath;
+        std::optional<teb_local_planner::Costmap> teb_costmap;
         std::unique_ptr<teb_local_planner::HomotopyClassPlanner> hcp_{nullptr};
     };
 } // namespace armarx::navigation::local_planning

source/armarx/navigation/local_planning/ros_conversions.cpp

@@ -164,4 +164,29 @@ namespace armarx::navigation::conv
         return {.a = ellipse.b / 1000, .b = ellipse.a / 1000}; // [mm] to [m]
     }
 
+    teb_local_planner::Costmap
+    toRos(const algorithms::Costmap& costmap)
+    {
+        const algorithms::Costmap::Parameters& params = costmap.params();
+        const teb_local_planner::Costmap::Parameters teb_params = {
+            params.binaryGrid, params.cellSize / 1000}; // [mm] to [m]
+
+        const algorithms::SceneBounds& bounds = costmap.getLocalSceneBounds();
+        const teb_local_planner::Costmap::SceneBounds teb_bounds = {
+            toRos2D(bounds.min).cast<float>(), toRos2D(bounds.max).cast<float>()};
+
+        const std::optional<algorithms::Costmap::Mask>& mask = costmap.getMask();
+        boost::optional<teb_local_planner::Costmap::Mask> teb_mask = boost::none;
+        if (mask)
+        {
+            teb_mask = mask.value();
+        }
+
+        teb_local_planner::Costmap::Pose2D teb_origin = costmap.origin();
+        teb_origin.translation() /= 1000; // [mm] to[m]
+
+        return teb_local_planner::Costmap{
+            costmap.getGrid(), teb_params, teb_bounds, teb_mask, teb_origin};
+    }
+
 } // namespace armarx::navigation::conv

source/armarx/navigation/local_planning/ros_conversions.h

@@ -27,6 +27,8 @@
 #include <geometry_msgs/Twist.h>
 #include <teb_local_planner/pose_se2.h>
 #include <teb_local_planner/timed_elastic_band.h>
+#include <teb_local_planner/costmap.h>
+#include <armarx/navigation/algorithms/Costmap.h>
 
 namespace armarx::navigation::conv
 {
@@ -49,4 +51,6 @@ namespace armarx::navigation::conv
 
     human::shapes::Ellipse toRos(const human::shapes::Ellipse& ellipse);
 
+    teb_local_planner::Costmap toRos(const algorithms::Costmap& costmap);
+
 } // namespace armarx::navigation::conv