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] 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