From b284e3a29289301a4f3cd02e0302c9c90fabae93 Mon Sep 17 00:00:00 2001
From: Lennart Grosskreutz <lennart.grosskreutz@student.kit.edu>
Date: Mon, 29 Aug 2022 18:46:17 +0200
Subject: [PATCH 1/5] Provide costmap to homotopy class planner
---
source/armarx/navigation/algorithms/Costmap.h | 6 +++--
.../navigation/local_planning/CMakeLists.txt | 1 +
.../local_planning/TimedElasticBands.cpp | 21 ++++++++++++++--
.../local_planning/TimedElasticBands.h | 2 ++
.../local_planning/ros_conversions.cpp | 25 +++++++++++++++++++
.../local_planning/ros_conversions.h | 4 +++
6 files changed, 55 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 6f452b7d..d168ef7d 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/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);
}
diff --git a/source/armarx/navigation/local_planning/CMakeLists.txt b/source/armarx/navigation/local_planning/CMakeLists.txt
index ae184519..4b5d36b1 100644
--- a/source/armarx/navigation/local_planning/CMakeLists.txt
+++ b/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
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 91483e74..1b86f5d3 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/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
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 9eba4d52..1c1db279 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/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
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 86fd6a2c..91736c32 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/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
diff --git a/source/armarx/navigation/local_planning/ros_conversions.h b/source/armarx/navigation/local_planning/ros_conversions.h
index 548dce73..05b52e77 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.h
+++ b/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
--
GitLab
From fc5d357f69845c6f0bbe83c9bda51986c7dd7dd6 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:06:55 +0200
Subject: [PATCH 2/5] Fix costmap conversion
Convert armarx costmap from mm to m to teb.
---
.../local_planner_config/TimedElasticBands/default.json | 2 +-
source/armarx/navigation/local_planning/ros_conversions.cpp | 6 ++++--
2 files changed, 5 insertions(+), 3 deletions(-)
diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 34d3506a..4265bf31 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -6,7 +6,7 @@
"pse": {
"pse_costum_obstacle_penalties": true,
"pse_costum_obstacle_penalties_dynamic": true,
- "weight_costmap": 0.5,
+ "weight_costmap": 1,
"weight_global_path_position": 0.3,
"weight_global_path_orientation": 0.3,
"lrk_use_alternative_approach": false,
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 91736c32..2303b64c 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -185,8 +185,10 @@ namespace armarx::navigation::conv
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};
+ algorithms::Costmap::Grid teb_grid = costmap.getGrid();
+ teb_grid /= 1000; // [mm] to [m]
+
+ return teb_local_planner::Costmap{teb_grid, teb_params, teb_bounds, teb_mask, teb_origin};
}
} // namespace armarx::navigation::conv
--
GitLab
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 3/5] 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
From 2188d923d6e63466058a81289a31ca9eafb70d50 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:16:28 +0200
Subject: [PATCH 4/5] Add costmap with custom penalty to optimization
---
.../navigation/human/ProxemicZoneCreator.cpp | 15 ++++++++++-----
.../local_planning/TebObstacleManager.cpp | 1 +
.../local_planning/TimedElasticBands.cpp | 13 ++++++++++---
3 files changed, 21 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.cpp b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
index 8a6aac53..acd453fa 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.cpp
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
@@ -9,10 +9,13 @@ namespace armarx::navigation::human
ProxemicZoneCreator::createProxemicZones(const Human& human)
{
// intimate zone
- ProxemicZone intimate{.pose = human.pose,
- .shape = {.a = params.intimateRadius, .b = params.intimateRadius},
- .penalty = intimatePenalty,
- .weight = params.intimateWeight};
+ ProxemicZone intimate{
+ .pose = human.pose,
+ .shape = {.a = params.intimateRadius, .b = params.intimateRadius},
+ .penalty = intimatePenalty,
+ .weight = params.intimateWeight,
+ .homotopicRelevance = true,
+ };
// personal zone
auto& global_R_human = human.pose.linear();
@@ -38,7 +41,9 @@ namespace armarx::navigation::human
.pose = pose,
.shape = {.a = params.personalRadius, .b = movementStretch * params.personalRadius},
.penalty = personalPenalty,
- .weight = params.personalWeight};
+ .weight = params.personalWeight,
+ .homotopicRelevance = false,
+ };
return {intimate, personal};
}
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index add2038f..e2402e12 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -37,6 +37,7 @@ namespace armarx::navigation::local_planning
obst->pushBackVertex(max.x(), min.y());
obst->finalizePolygon();
+ obst->setUseForOptimization(false);
container.push_back(obst);
// visualize bounding box if layer is available
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 7bd970f0..8f86171a 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -1,4 +1,5 @@
#include "TimedElasticBands.h"
+
#include <optional>
#include <SimoxUtility/algorithm/apply.hpp>
@@ -68,10 +69,16 @@ 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) {
+ if (teb_costmap)
+ {
+ // TODO: where to put all the parameters
+ const human::ExponentialPenaltyModel penalty{
+ .minDistance = 0.5, .epsilon = 0, .exponent = 1.2};
+
+ teb_costmap->setPenaltyModel(conv::toRos(penalty));
hcp_->setCostmap(&teb_costmap.value());
}
ros::Time::init(); // we have to init time before we can use the planner
@@ -130,7 +137,7 @@ namespace armarx::navigation::local_planning
return {};
}
- if(hcp_->getTrajectoryContainer().empty())
+ if (hcp_->getTrajectoryContainer().empty())
{
ARMARX_VERBOSE << "Did not find any trajectory!";
return std::nullopt;
--
GitLab
From 84cb65920cff918e75c0af61451318bee2c309c2 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 26 Sep 2022 09:55:47 +0200
Subject: [PATCH 5/5] minor fixes (code style)
---
.../navigation/local_planning/TimedElasticBands.cpp | 10 ++++++++--
1 file changed, 8 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 8f86171a..86f63c95 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -221,10 +221,16 @@ namespace armarx::navigation::local_planning
void
TimedElasticBands::setTebCostmap()
{
- if (!scene.staticScene)
+ if (not scene.staticScene.has_value())
+ {
return;
- if (!scene.staticScene->distanceToObstaclesCostmap)
+ }
+
+ if (not scene.staticScene->distanceToObstaclesCostmap.has_value())
+ {
return;
+ }
+
const algorithms::Costmap& navigationCostmap =
scene.staticScene->distanceToObstaclesCostmap.value();
teb_costmap.emplace(conv::toRos(navigationCostmap));
--
GitLab