diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 34d3506a8d9a815e1cc133d7b80d86ef6b335da0..4265bf31c32a661a71ca7e2b701dad40b035d881 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/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 6f452b7dcc5e0fb3f8265f344c54b6b3701cd13b..d168ef7d9c2973d7e55bf045d304d892c87afb66 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/human/ProxemicZoneCreator.cpp b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
index 8a6aac53e45052b0359a4a9a13ba98df51aa15ba..acd453fa124371ac38a5438bed46386fc7f6978a 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/human/types.h b/source/armarx/navigation/human/types.h
index fe5490d55df1235f0e5a0992d329f5b2a41c7331..0c3e08043b9fcea45aa603a0ef01d0eeba87f63d 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/CMakeLists.txt b/source/armarx/navigation/local_planning/CMakeLists.txt
index ae184519cebee4c6ee46881b72c09702441377df..4b5d36b1235915c6e8ac01bc3b3a57a76dfb5fd3 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/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 8f82f74d2657f27fa47d28f5a9fac7f0ae183f60..e2402e121d279599b6968bf8aec2063749b49f5d 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
@@ -70,9 +71,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 +86,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/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index c67b6256f5f601490a51f6bd3aad0ca4d1a8aecd..86f63c95260b97f9a963968ed638fcd42d65daec 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>
@@ -69,7 +70,17 @@ 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);
+ //set member teb_costmap
+ setTebCostmap();
+ 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
}
@@ -126,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;
@@ -206,4 +217,23 @@ 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 (not scene.staticScene.has_value())
+ {
+ return;
+ }
+
+ if (not scene.staticScene->distanceToObstaclesCostmap.has_value())
+ {
+ 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 9eba4d52739af83a79e0adcbc6cd8b0b19386b27..1c1db279a8bf343adccf20a8cc4ac67843d46180 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 86fd6a2c954ab6a9d44a44e7255eea850b2d1711..657db4c48edb26dd1cbbba13282e7344dfad4777 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -164,4 +164,46 @@ 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]
+
+ 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};
+ }
+
+ 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 548dce7300dd93a373e92fff9dc4701667449b1c..083ddcc57b2e78074c06428ce31acaf06e343514 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.h
+++ b/source/armarx/navigation/local_planning/ros_conversions.h
@@ -21,10 +21,13 @@
#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>
@@ -49,4 +52,10 @@ namespace armarx::navigation::conv
human::shapes::Ellipse toRos(const human::shapes::Ellipse& ellipse);
+ 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