From 7e8ee33076517a72f416a4c2c77b5c52f7d0a272 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 22 Aug 2022 14:11:42 +0200
Subject: [PATCH 1/3] Visualize path alternatives inside TimedElasticBands
---
.../local_planning/TimedElasticBands.cpp | 29 ++++++++++++++++++-
1 file changed, 28 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index b9a9d0b3..ce1fbbd8 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -1,5 +1,6 @@
#include "TimedElasticBands.h"
+#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
@@ -128,6 +129,32 @@ namespace armarx::navigation::local_planning
core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
+ // visualize path alternatives
+ if (arviz != nullptr)
+ {
+ auto layer = arviz->layer("local_planner_path_alternatives");
+
+ int i = 0;
+ int bestTebIdx = hcp_->bestTebIdx();
+ for (const auto& teb : hcp_->getTrajectoryContainer())
+ {
+ if (i == bestTebIdx)
+ {
+ continue;
+ }
+ const core::LocalTrajectory trajectory = conv::fromRos(teb->teb());
+ const std::vector<Eigen::Vector3f> points =
+ simox::alg::apply(trajectory.points(),
+ [](const core::LocalTrajectoryPoint& pt) -> Eigen::Vector3f
+ { return pt.pose.translation(); });
+ layer.add(viz::Path("path_alternative_" + std::to_string(i))
+ .points(points)
+ .color(simox::Color::gray()));
+ i++;
+ }
+ arviz->commit(layer);
+ }
+
return {{.trajectory = best}};
}
@@ -165,7 +192,7 @@ namespace armarx::navigation::local_planning
ARMARX_CHECK(scene.dynamicScene.has_value());
for (const auto& obst : scene.dynamicScene.value().humans)
{
- obstManager.addHumanObstacle(obst, visPtr);
+ obstManager.addHumanObstacle(obst, visPtr);
}
}
--
GitLab
From 763f18ff43d84ad417771cf985a3ecaa87b7385e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 22 Aug 2022 17:32:50 +0200
Subject: [PATCH 2/3] Move visualization member to LocalPlanner
viz::ScopedClient was changed to viz::Client
---
source/armarx/navigation/local_planning/LocalPlanner.cpp | 6 ++++++
source/armarx/navigation/local_planning/LocalPlanner.h | 7 ++++++-
.../navigation/local_planning/TimedElasticBands.cpp | 9 +--------
.../armarx/navigation/local_planning/TimedElasticBands.h | 5 -----
4 files changed, 13 insertions(+), 14 deletions(-)
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.cpp b/source/armarx/navigation/local_planning/LocalPlanner.cpp
index b98a0a53..4b7ea7ed 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.cpp
+++ b/source/armarx/navigation/local_planning/LocalPlanner.cpp
@@ -5,4 +5,10 @@ namespace armarx::navigation::local_planning
LocalPlanner::LocalPlanner(const core::Scene& context) : context(context)
{
}
+
+ void
+ LocalPlanner::setVisualization(viz::Client* vis)
+ {
+ arviz = 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 8c2554d6..f2f6641b 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.h
+++ b/source/armarx/navigation/local_planning/LocalPlanner.h
@@ -26,13 +26,14 @@
#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <armarx/navigation/local_planning/core.h>
#include <armarx/navigation/core/DynamicScene.h>
#include <armarx/navigation/core/StaticScene.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/local_planning/core.h>
namespace armarx::navigation::local_planning
{
@@ -57,7 +58,11 @@ namespace armarx::navigation::local_planning
virtual std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) = 0;
+ void setVisualization(viz::Client* vis);
+
protected:
+ viz::Client* 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 b9a9d0b3..0a580d18 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -131,13 +131,6 @@ namespace armarx::navigation::local_planning
return {{.trajectory = best}};
}
- void
- TimedElasticBands::setVisualization(viz::ScopedClient* vis)
- {
- arviz = vis;
- }
-
-
void
TimedElasticBands::fillObstacles()
{
@@ -165,7 +158,7 @@ namespace armarx::navigation::local_planning
ARMARX_CHECK(scene.dynamicScene.has_value());
for (const auto& obst : scene.dynamicScene.value().humans)
{
- obstManager.addHumanObstacle(obst, visPtr);
+ obstManager.addHumanObstacle(obst, visPtr);
}
}
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 2bf54057..9eba4d52 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -22,7 +22,6 @@
#pragma once
-#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include "armarx/navigation/core/basic_types.h"
@@ -59,8 +58,6 @@ namespace armarx::navigation::local_planning
std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
- void setVisualization(viz::ScopedClient* vis);
-
private:
void readDefaultConfig(arondto::TimedElasticBandsParams& target);
void fillObstacles();
@@ -76,7 +73,5 @@ namespace armarx::navigation::local_planning
TebObstacleManager obstManager;
teb_local_planner::PoseSequence teb_globalPath;
std::unique_ptr<teb_local_planner::HomotopyClassPlanner> hcp_{nullptr};
-
- viz::ScopedClient* arviz;
};
} // namespace armarx::navigation::local_planning
--
GitLab
From 9c99adbbc5bc794be1a559cd28aaefd3c55e4702 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 22 Aug 2022 17:33:51 +0200
Subject: [PATCH 3/3] Set visualization of LocalPlanner in createConfig
---
.../components/navigator/Component.cpp | 27 ++++++++++---------
1 file changed, 15 insertions(+), 12 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 402c90e4..34d27ae7 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -67,9 +67,6 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <armarx/navigation/memory/client/costmap/Reader.h>
-#include <armarx/navigation/server/execution/ExecutorInterface.h>
-#include <armarx/navigation/server/scene_provider/SceneProvider.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/astar/util.h>
@@ -84,9 +81,12 @@
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
+#include <armarx/navigation/server/execution/ExecutorInterface.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
+#include <armarx/navigation/server/scene_provider/SceneProvider.h>
#include <armarx/navigation/util/util.h>
namespace armarx::navigation::components::navigator
@@ -257,6 +257,9 @@ namespace armarx::navigation::components::navigator
server::NavigationStack stack =
fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
+ // set visualization of LocalPlanner
+ stack.localPlanner->setVisualization(&getArvizClient());
+
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
@@ -278,12 +281,12 @@ namespace armarx::navigation::components::navigator
std::forward_as_tuple(
server::Navigator::Config{.stack = std::move(stack),
.general = server::Navigator::Config::General{}},
- server::Navigator::InjectedServices{.executor = executorPtr,
- .publisher =
- memoryPublishers.at(callerId).get(),
- .introspector = &(introspector.value()),
- .debugObserverHelper = &getDebugObserverComponentPlugin(),
- .sceneProvider = &sceneProvider.value()}));
+ server::Navigator::InjectedServices{
+ .executor = executorPtr,
+ .publisher = memoryPublishers.at(callerId).get(),
+ .introspector = &(introspector.value()),
+ .debugObserverHelper = &getDebugObserverComponentPlugin(),
+ .sceneProvider = &sceneProvider.value()}));
}
void
@@ -319,7 +322,7 @@ namespace armarx::navigation::components::navigator
Component::updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
const std::string& navigationMode,
const std::string& callerId,
- const Ice::Current& /*c*/)
+ const Ice::Current& /*c*/)
{
ARMARX_TRACE;
@@ -535,9 +538,9 @@ namespace armarx::navigation::components::navigator
server::Navigator*
Component::activeNavigator()
{
-
+
const auto navigatorId = activeNavigatorId();
- if(not navigatorId.has_value())
+ if (not navigatorId.has_value())
{
return nullptr;
}
--
GitLab