Add penalty model conversions

source/armarx/navigation/human/types.h

@@ -57,6 +57,12 @@ namespace armarx::navigation::human
 
     using HumanGroups = std::vector<HumanGroup>;
 
+    struct LinearPenaltyModel
+    {
+        float minDistance; // [m]
+        float epsilon; // [m]
+    };
+
     struct ExponentialPenaltyModel
     {
         float minDistance; // [m]

source/armarx/navigation/local_planning/TebObstacleManager.cpp

@@ -70,9 +70,7 @@ namespace armarx::navigation::local_planning
 
             const auto& penalty = proxemicZone.penalty;
 
-            obst->setPenaltyModel(boost::make_shared<teb_local_planner::ExponentialPenaltyModel>(
-                teb_local_planner::LinearPenaltyModel(penalty.minDistance, penalty.epsilon),
-                penalty.exponent));
+            obst->setPenaltyModel(conv::toRos(penalty));
 
             obst->setWeight(proxemicZone.weight);
             obst->setHomotopicRelevance(proxemicZone.homotopicRelevance);
@@ -87,7 +85,8 @@ namespace armarx::navigation::local_planning
             // visualize proxemic zone if layer is available
             if (visLayer != nullptr)
             {
-                const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 10.f-i);
+                const Eigen::Vector3f axisLength(
+                    proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
                 const core::Pose pose3d = conv::to3D(proxemicZone.pose);
 
                 visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))

source/armarx/navigation/local_planning/ros_conversions.cpp

@@ -191,4 +191,19 @@ namespace armarx::navigation::conv
         return teb_local_planner::Costmap{teb_grid, teb_params, teb_bounds, teb_mask, teb_origin};
     }
 
+    teb_local_planner::PenaltyModelPtr
+    toRos(const human::LinearPenaltyModel& model)
+    {
+        return boost::make_shared<teb_local_planner::LinearPenaltyModel>(
+            teb_local_planner::LinearPenaltyModel(model.minDistance, model.epsilon));
+    }
+
+    teb_local_planner::PenaltyModelPtr
+    toRos(const human::ExponentialPenaltyModel& model)
+    {
+        return boost::make_shared<teb_local_planner::ExponentialPenaltyModel>(
+            teb_local_planner::LinearPenaltyModel(model.minDistance, model.epsilon),
+            model.exponent);
+    }
+
 } // namespace armarx::navigation::conv

source/armarx/navigation/local_planning/ros_conversions.h

@@ -21,14 +21,15 @@
 
 #pragma once
 
+#include <armarx/navigation/algorithms/Costmap.h>
 #include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/core/types.h>
 #include <armarx/navigation/human/shapes.h>
 #include <geometry_msgs/Twist.h>
+#include <teb_local_planner/costmap.h>
+#include <teb_local_planner/penalty_models.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
 {
@@ -53,4 +54,8 @@ namespace armarx::navigation::conv
 
     teb_local_planner::Costmap toRos(const algorithms::Costmap& costmap);
 
+    teb_local_planner::PenaltyModelPtr toRos(const human::LinearPenaltyModel& model);
+
+    teb_local_planner::PenaltyModelPtr toRos(const human::ExponentialPenaltyModel& model);
+
 } // namespace armarx::navigation::conv