From 90183c5eab3ac760636bcc2e3206719f65bbb9ab Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 25 Aug 2022 14:24:14 +0200
Subject: [PATCH 1/7] Make visualization inside LocalPlanner ScopedClient
---
.../navigation/components/navigator/Component.cpp | 2 +-
.../navigation/local_planning/LocalPlanner.cpp | 4 ++--
.../navigation/local_planning/LocalPlanner.h | 4 ++--
.../local_planning/TimedElasticBands.cpp | 15 ++++++++-------
4 files changed, 13 insertions(+), 12 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 34d27ae7..3509d2f3 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -258,7 +258,7 @@ namespace armarx::navigation::components::navigator
fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
// set visualization of LocalPlanner
- stack.localPlanner->setVisualization(&getArvizClient());
+ stack.localPlanner->setVisualization(getArvizClient());
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.cpp b/source/armarx/navigation/local_planning/LocalPlanner.cpp
index 4b7ea7ed..ab0fbd22 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.cpp
+++ b/source/armarx/navigation/local_planning/LocalPlanner.cpp
@@ -7,8 +7,8 @@ namespace armarx::navigation::local_planning
}
void
- LocalPlanner::setVisualization(viz::Client* vis)
+ LocalPlanner::setVisualization(viz::Client& vis)
{
- arviz = vis;
+ arviz.emplace(vis);
}
} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.h b/source/armarx/navigation/local_planning/LocalPlanner.h
index f2f6641b..03131881 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.h
+++ b/source/armarx/navigation/local_planning/LocalPlanner.h
@@ -58,10 +58,10 @@ namespace armarx::navigation::local_planning
virtual std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) = 0;
- void setVisualization(viz::Client* vis);
+ void setVisualization(viz::Client& vis);
protected:
- viz::Client* arviz;
+ std::optional<viz::ScopedClient> arviz;
private:
const core::Scene& context;
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 2633583d..daedf66c 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -130,9 +130,9 @@ namespace armarx::navigation::local_planning
core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
// visualize path alternatives
- if (arviz != nullptr)
+ if (arviz)
{
- auto layer = arviz->layer("local_planner_path_alternatives");
+ auto layer = arviz.value().layer("local_planner_path_alternatives");
int i = 0;
int bestTebIdx = hcp_->bestTebIdx();
@@ -152,7 +152,8 @@ namespace armarx::navigation::local_planning
.color(simox::Color::gray()));
i++;
}
- arviz->commit(layer);
+
+ arviz.value().commit(layer);
}
return {{.trajectory = best}};
@@ -165,9 +166,9 @@ namespace armarx::navigation::local_planning
viz::Layer* visPtr = nullptr;
viz::Layer visLayer;
- if (arviz != nullptr)
+ if (arviz)
{
- visLayer = arviz->layer("local_planner_obstacles");
+ visLayer = arviz.value().layer("local_planner_obstacles");
visPtr = &visLayer;
}
@@ -189,9 +190,9 @@ namespace armarx::navigation::local_planning
}
}
- if (arviz != nullptr)
+ if (arviz)
{
- arviz->commit(visLayer);
+ arviz.value().commit(visLayer);
}
ARMARX_INFO << "TEB: added " << obstManager.size() << " obstacles";
--
GitLab
From ee0795dabf509edd05f37326b1786217bcc6cf38 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 25 Aug 2022 14:24:33 +0200
Subject: [PATCH 2/7] Fix bounding box coordinates
---
source/armarx/navigation/local_planning/TebObstacleManager.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 5e46a901..c7288854 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -29,7 +29,7 @@ namespace armarx::navigation::local_planning
auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
const Eigen::Vector2d min = conv::toRos(bbox.getMin());
- const Eigen::Vector2d max = conv::toRos(bbox.getMin());
+ const Eigen::Vector2d max = conv::toRos(bbox.getMax());
obst->pushBackVertex(min);
obst->pushBackVertex(min.x(), max.y());
--
GitLab
From 05569ad63d0ca019a46b1ed945b94177ecb1003f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 25 Aug 2022 17:20:18 +0200
Subject: [PATCH 3/7] Fix offset paramter
Only apply rotation to offset, not translation.
---
source/armarx/navigation/human/ProxemicZoneCreator.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.cpp b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
index e6495840..3e946a31 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.cpp
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
@@ -15,8 +15,8 @@ namespace armarx::navigation::human
.weight = params.intimateWeight};
// personal zone
- auto& global_T_human = human.pose;
- auto& global_V_offset = global_T_human * params.offset;
+ auto& global_R_human = human.pose.linear();
+ auto& global_V_offset = global_R_human * params.offset;
core::Pose2D pose = human.pose;
pose.translation() += global_V_offset;
--
GitLab
From a59775fe2903e2f114d4f1eaf4a4f69ff9652195 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 25 Aug 2022 17:22:26 +0200
Subject: [PATCH 4/7] Use velocity in m/s for stretching
---
source/armarx/navigation/human/ProxemicZoneCreator.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.cpp b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
index 3e946a31..8a6aac53 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.cpp
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
@@ -22,7 +22,8 @@ namespace armarx::navigation::human
pose.translation() += global_V_offset;
float velocityNorm = human.linearVelocity.norm();
- float movementStretch = 1 + velocityNorm * params.movementInfluence;
+ // stretch with velocity in m/s as factor
+ float movementStretch = 1 + velocityNorm * params.movementInfluence / 1000; // [mm] to [m]
if (velocityNorm != 0)
{
// y pointing forward
--
GitLab
From 1fd9cb56180d6231deb60b0c32c1862ac6149189 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 25 Aug 2022 17:39:53 +0200
Subject: [PATCH 5/7] Fix proxemic zone visualization, draw them in different
colors
Use the proper pose to draw the proxemic zone ellipse.
Use different colors for the intimate and personal zones.
---
.../armarx/navigation/local_planning/TebObstacleManager.cpp | 6 ++++--
.../armarx/navigation/local_planning/TebObstacleManager.h | 3 +++
2 files changed, 7 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index c7288854..d532e6d9 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -58,6 +58,7 @@ namespace armarx::navigation::local_planning
{
auto proxemicZones = proxemics.createProxemicZones(human);
+ int i = 0;
for (const auto& proxemicZone : proxemicZones)
{
auto pose = conv::toRos(proxemicZone.pose);
@@ -85,13 +86,14 @@ namespace armarx::navigation::local_planning
if (visLayer != nullptr)
{
const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0);
- const core::Pose pose3d = conv::to3D(human.pose);
+ const core::Pose pose3d = conv::to3D(proxemicZone.pose);
visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))
.pose(pose3d)
.axisLengths(axisLength)
- .color(simox::Color::blue()));
+ .color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
}
+ i++;
}
}
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.h b/source/armarx/navigation/local_planning/TebObstacleManager.h
index b336b65b..b8859c4b 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.h
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.h
@@ -50,6 +50,9 @@ namespace armarx::navigation::local_planning
teb_local_planner::ObstContainer& container;
human::ProxemicZoneCreator proxemics;
int visualizationIndex;
+
+ const std::array<simox::Color, 2> PROXEMIC_ZONE_COLOR = {simox::Color::red(),
+ simox::Color::blue()};
};
--
GitLab
From 0e2a263a86f90004dd99fa6183c24767af000e07 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 25 Aug 2022 18:20:57 +0200
Subject: [PATCH 6/7] Normalize angle in ros conversions
---
source/armarx/navigation/local_planning/ros_conversions.cpp | 2 ++
1 file changed, 2 insertions(+)
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 4bb21ef4..86fd6a2c 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -49,6 +49,8 @@ namespace armarx::navigation::conv
// we change it such that x is pointing forward.
// ROS: x pointing forward, ArmarX: y pointing forward
theta += M_PI_2;
+ // normalize angle
+ theta = g2o::normalize_theta(theta);
return {pos / 1000., theta}; // [mm] to [m]
}
--
GitLab
From 9286eebd79692745637d249723cc162356ae02c6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 25 Aug 2022 18:21:25 +0200
Subject: [PATCH 7/7] Add some comments
---
source/armarx/navigation/human/ProxemicZoneCreator.h | 4 +++-
.../armarx/navigation/local_planning/TebObstacleManager.cpp | 2 ++
source/armarx/navigation/local_planning/TimedElasticBands.cpp | 2 +-
3 files changed, 6 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.h b/source/armarx/navigation/human/ProxemicZoneCreator.h
index d1696db4..fed4c18c 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.h
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.h
@@ -38,7 +38,9 @@ namespace armarx::navigation::human
float personalRadius = 1000;
float movementInfluence = 1;
- // in the coordinate system of the human
+ // an offset applied to the personal zone in the coordinate system of the human
+ // a positive x-value means an offset to the right
+ // a positive y-value means an offset to the front
Eigen::Vector2f offset{100, 150};
};
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index d532e6d9..35d3afb4 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -39,6 +39,7 @@ namespace armarx::navigation::local_planning
obst->finalizePolygon();
container.push_back(obst);
+ // visualize bounding box if layer is available
if (visLayer != nullptr)
{
const Eigen::Vector3f min3d = conv::fromRos(min);
@@ -83,6 +84,7 @@ namespace armarx::navigation::local_planning
container.push_back(obst);
+ // visualize proxemic zone if layer is available
if (visLayer != nullptr)
{
const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0);
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index daedf66c..91483e74 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -112,7 +112,7 @@ namespace armarx::navigation::local_planning
geometry_msgs::Twist velocity_start = conv::toRos(scene.platformVelocity);
- //TODO (SALt): fill obstacles
+
fillObstacles();
try
--
GitLab