From b027fe6edc73e6dfcba4007e3423b47d7c9580c4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 26 Aug 2022 14:46:42 +0200
Subject: [PATCH] Add draft of human group obstacle implementation
---
.../local_planning/TebObstacleManager.cpp | 36 +++++++++++++++++++
.../local_planning/TebObstacleManager.h | 1 +
2 files changed, 37 insertions(+)
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 5e46a901..5f246ace 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -95,4 +95,40 @@ namespace armarx::navigation::local_planning
}
}
+ void
+ TebObstacleManager::addHumanGroupObstacle(const human::HumanGroup& group, viz::Layer* visLayer)
+ {
+ auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
+
+
+ for (const Eigen::Vector2f& vertex : group.shape.vertices)
+ {
+ const Eigen::Vector2d ver = conv::toRos2D(vertex);
+ obst->pushBackVertex(ver);
+ }
+
+ obst->finalizePolygon();
+ container.push_back(obst);
+
+ if (visLayer != nullptr)
+ {
+ for (const human::Human& human : group.humans)
+ {
+ const auto humanPos = conv::to3D(human.pose).translation();
+ visLayer->add(viz::Cylinder("grouped_human_" + std::to_string(visualizationIndex++))
+ .fromTo(humanPos, humanPos + Eigen::Vector3f{0, 0, 10})
+ .radius(200)
+ .color(simox::Color::red()));
+ }
+
+ viz::Polygon poly{"human_group_" + std::to_string(visualizationIndex++)};
+ for (const Eigen::Vector2f& vertex : group.shape.vertices)
+ {
+ poly.addPoint(conv::to3D(vertex));
+ }
+ poly.color(simox::Color::yellow());
+ visLayer->add(poly);
+ }
+ }
+
} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.h b/source/armarx/navigation/local_planning/TebObstacleManager.h
index b336b65b..39a861a7 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.h
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.h
@@ -45,6 +45,7 @@ namespace armarx::navigation::local_planning
void addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer = nullptr);
void addHumanObstacle(const human::Human& human, viz::Layer* visLayer = nullptr);
+ void addHumanGroupObstacle(const human::HumanGroup& group, viz::Layer* visLayer = nullptr);
private:
teb_local_planner::ObstContainer& container;
--
GitLab