From da60b8ca1bbdce1c7ce451ce632b0f08851283cf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 22 Sep 2022 19:15:21 +0200
Subject: [PATCH] Add penalty model conversions
---
source/armarx/navigation/human/types.h | 6 ++++++
.../local_planning/TebObstacleManager.cpp | 7 +++----
.../navigation/local_planning/ros_conversions.cpp | 15 +++++++++++++++
.../navigation/local_planning/ros_conversions.h | 9 +++++++--
4 files changed, 31 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index fe5490d5..0c3e0804 100644
--- a/source/armarx/navigation/human/types.h
+++ b/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]
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 8f82f74d..add2038f 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/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++))
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 2303b64c..657db4c4 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/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
diff --git a/source/armarx/navigation/local_planning/ros_conversions.h b/source/armarx/navigation/local_planning/ros_conversions.h
index 05b52e77..083ddcc5 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.h
+++ b/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
--
GitLab