From 4f4212e5f9b7d08976ef87620843f53d58719f54 Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Tue, 26 Oct 2021 11:09:46 +0200
Subject: [PATCH 01/16] Refactors ComponentPlugin to be self-sufficient
---
.../navigation/client/ComponentPlugin.cpp | 26 ++++++------
.../navigation/client/ComponentPlugin.h | 40 +++++++++----------
.../components/example_client/Component.cpp | 30 +++++++-------
3 files changed, 46 insertions(+), 50 deletions(-)
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index 3578b695..07cb43c0 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -36,16 +36,23 @@ armarx::navigation::client::ComponentPlugin::preOnConnectComponent()
ARMARX_TRACE;
const std::string componentName = parent().getName();
- IceNavigator& srv = parent<armarx::navigation::client::ComponentPluginUser>().iceNavigator;
ARMARX_CHECK_NOT_NULL(navigatorPrx) << "Navigator proxy is null!";
- srv.setNavigatorComponent(navigatorPrx);
+ iceNavigator.setNavigatorComponent(navigatorPrx);
+}
+
+void
+armarx::navigation::client::ComponentPlugin::configureNavigator(
+ const client::NavigationStackConfig& stackConfig,
+ const std::string& configId)
+{
+ ARMARX_TRACE;
+ iceNavigator.createConfig(stackConfig, configId);
}
// ComponentPluginUser
-armarx::navigation::client::ComponentPluginUser::ComponentPluginUser() :
- navigator{navigatorServices}
+armarx::navigation::client::ComponentPluginUser::ComponentPluginUser()
{
ARMARX_TRACE;
addPlugin(plugin);
@@ -55,15 +62,8 @@ void
armarx::navigation::client::ComponentPluginUser::configureNavigator(
const client::NavigationStackConfig& stackConfig)
{
- configureNavigator(stackConfig, getName());
-}
-
-void
-armarx::navigation::client::ComponentPluginUser::configureNavigator(
- const client::NavigationStackConfig& stackConfig,
- const std::string& configId)
-{
- iceNavigator.createConfig(stackConfig, configId);
+ ARMARX_TRACE;
+ plugin->configureNavigator(stackConfig, getName());
}
armarx::navigation::client::ComponentPluginUser::~ComponentPluginUser() = default;
diff --git a/source/armarx/navigation/client/ComponentPlugin.h b/source/armarx/navigation/client/ComponentPlugin.h
index f64ce55b..ce6c5ad8 100644
--- a/source/armarx/navigation/client/ComponentPlugin.h
+++ b/source/armarx/navigation/client/ComponentPlugin.h
@@ -14,11 +14,13 @@
namespace armarx::navigation::client
{
- class ComponentPlugin : virtual public armarx::ComponentPlugin
+ class ComponentPlugin : public armarx::ComponentPlugin
{
public:
- using armarx::ComponentPlugin::ComponentPlugin;
+ ComponentPlugin(ManagedIceObject& parent, const std::string& prefix) :
+ armarx::ComponentPlugin(parent, prefix), navigator(navigatorServices){};
+// using armarx::ComponentPlugin::ComponentPlugin;
~ComponentPlugin() override;
@@ -28,10 +30,21 @@ namespace armarx::navigation::client
void preOnConnectComponent() override;
+ void configureNavigator(const client::NavigationStackConfig& stackConfig,
+ const std::string& configId);
+
private:
static constexpr const char* PROPERTY_NAME = "nav.NavigatorName";
client::NavigatorInterfacePrx navigatorPrx;
+ IceNavigator iceNavigator;
+ // MemorySubscriber memorySubscriber;
+ SimpleEventHandler memorySubscriber; // FIXME
+ Navigator::InjectedServices navigatorServices{.navigator = &iceNavigator,
+ .subscriber = &memorySubscriber};
+
+ public:
+ Navigator navigator;
};
@@ -42,31 +55,14 @@ namespace armarx::navigation::client
ComponentPluginUser();
void configureNavigator(const client::NavigationStackConfig& stackConfig);
-
- void configureNavigator(const client::NavigationStackConfig& stackConfig,
- const std::string& configId);
-
-
+ Navigator& getNavigator() {
+ return plugin->navigator;
+ }
// Non-API
-
- public:
~ComponentPluginUser() override;
- friend class ComponentPlugin;
-
private:
- IceNavigator iceNavigator;
- // MemorySubscriber memorySubscriber;
- SimpleEventHandler memorySubscriber; // FIXME
- Navigator::InjectedServices navigatorServices{.navigator = &iceNavigator,
- .subscriber = &memorySubscriber};
-
-
ComponentPlugin* plugin = nullptr;
-
-
- public:
- Navigator navigator;
};
} // namespace armarx::navigation::client
diff --git a/source/armarx/navigation/components/example_client/Component.cpp b/source/armarx/navigation/components/example_client/Component.cpp
index 1a8b1fdd..5b702a53 100644
--- a/source/armarx/navigation/components/example_client/Component.cpp
+++ b/source/armarx/navigation/components/example_client/Component.cpp
@@ -103,10 +103,10 @@ namespace armarx::navigation::components::example_client
.trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
// Example of registering a lambda as callback.
- navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
+ getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
// Example of registering a method as callback.
- navigator.onGoalReached([this] { goalReached(); });
+ getNavigator().onGoalReached([this] { goalReached(); });
std::this_thread::sleep_for(1s);
@@ -115,12 +115,12 @@ namespace armarx::navigation::components::example_client
core::Pose goal = core::Pose::Identity();
goal.translation() << 2000, 1000, 0;
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
+ getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
std::this_thread::sleep_for(15s);
// Wait until goal is reached
- armarx::navigation::client::StopEvent se = navigator.waitForStop();
+ armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
if (se)
{
ARMARX_INFO << "Goal 1 reached.";
@@ -143,12 +143,12 @@ namespace armarx::navigation::components::example_client
}
goal.translation() << -1500, 1000, 0;
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
+ getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
std::this_thread::sleep_for(15s);
// Wait until goal is reached
- se = navigator.waitForStop();
+ se = getNavigator().waitForStop();
if (se)
{
ARMARX_INFO << "Goal 2 reached.";
@@ -159,10 +159,10 @@ namespace armarx::navigation::components::example_client
}
goal.translation() << 4500, 4500, 0;
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
+ getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
// Wait until goal is reached
- se = navigator.waitForStop();
+ se = getNavigator().waitForStop();
if (se)
{
ARMARX_INFO << "Goal 3 reached.";
@@ -178,12 +178,12 @@ namespace armarx::navigation::components::example_client
ARMARX_INFO << "Moving into certain direction.";
// Start moving towards a direction
- navigator.moveTowards(core::Direction(100, 100, 0), core::NavigationFrame::Relative);
+ getNavigator().moveTowards(core::Direction(100, 100, 0), core::NavigationFrame::Relative);
std::this_thread::sleep_for(3s);
ARMARX_INFO << "Pausing movement.";
- navigator.pause();
+ getNavigator().pause();
}
void
@@ -206,10 +206,10 @@ namespace armarx::navigation::components::example_client
.trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
// Example of registering a lambda as callback.
- navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
+ getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
// Example of registering a method as callback.
- navigator.onGoalReached([this] { goalReached(); });
+ getNavigator().onGoalReached([this] { goalReached(); });
std::this_thread::sleep_for(1s);
@@ -232,12 +232,12 @@ namespace armarx::navigation::components::example_client
// clang-format on
ARMARX_INFO << "Starting execution";
- navigator.moveTo(pathBuilder, core::NavigationFrame::Absolute);
+ getNavigator().moveTo(pathBuilder, core::NavigationFrame::Absolute);
std::this_thread::sleep_for(25s); // TODO(fabian.reister): remove in the future
// Wait until goal is reached
- armarx::navigation::client::StopEvent se = navigator.waitForStop();
+ armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
if (se)
{
ARMARX_INFO << "Goal 1 reached.";
@@ -260,7 +260,7 @@ namespace armarx::navigation::components::example_client
}
ARMARX_INFO << "Pausing movement.";
- navigator.pause();
+ getNavigator().pause();
}
--
GitLab
From 9155dc5a583189058a1cab5c06a1a76a6f038446 Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Tue, 26 Oct 2021 11:10:21 +0200
Subject: [PATCH 02/16] Improves debugging output with ARMARX_TRACE
---
source/armarx/navigation/client/Navigator.cpp | 12 ++++++++++++
.../navigation/client/services/IceNavigator.cpp | 1 +
.../navigation/components/Navigator/Navigator.cpp | 9 ++++++++-
3 files changed, 21 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/client/Navigator.cpp b/source/armarx/navigation/client/Navigator.cpp
index 8e8ac4ce..9780c8c0 100644
--- a/source/armarx/navigation/client/Navigator.cpp
+++ b/source/armarx/navigation/client/Navigator.cpp
@@ -3,6 +3,7 @@
#include <algorithm>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/ice_conversions.h>
@@ -12,6 +13,7 @@ namespace armarx::navigation::client
Navigator::Navigator(const InjectedServices& services) : srv{services}
{
+ ARMARX_TRACE;
// these checks cannot be used when component is started in property-readout mode
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
ARMARX_CHECK_NOT_NULL(srv.subscriber) << "Subscriber service must not be null!";
@@ -28,6 +30,7 @@ namespace armarx::navigation::client
void
Navigator::moveTo(const core::Pose& pose, core::NavigationFrame frame)
{
+ ARMARX_TRACE;
moveTo(std::vector<core::Pose>{pose}, frame);
}
@@ -35,6 +38,7 @@ namespace armarx::navigation::client
void
Navigator::moveTo(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->moveTo(waypoints, frame);
}
@@ -43,6 +47,7 @@ namespace armarx::navigation::client
void
Navigator::moveTo(const PathBuilder& builder, core::NavigationFrame frame)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
const std::vector<WaypointTarget>& path = builder.path();
@@ -55,6 +60,7 @@ namespace armarx::navigation::client
void
Navigator::moveTowards(const core::Direction& direction, core::NavigationFrame frame)
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->moveTowards(direction, frame);
}
@@ -63,6 +69,7 @@ namespace armarx::navigation::client
void
Navigator::pause()
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->pause();
}
@@ -71,6 +78,7 @@ namespace armarx::navigation::client
void
Navigator::resume()
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->resume();
}
@@ -79,6 +87,7 @@ namespace armarx::navigation::client
void
Navigator::stop()
{
+ ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
srv.navigator->stop();
}
@@ -87,6 +96,7 @@ namespace armarx::navigation::client
void
Navigator::onGoalReached(const std::function<void(void)>& callback)
{
+ ARMARX_TRACE;
onGoalReached([&callback](const core::GoalReachedEvent&) { callback(); });
}
@@ -106,6 +116,7 @@ namespace armarx::navigation::client
StopEvent
Navigator::waitForStop()
{
+ ARMARX_TRACE;
std::unique_lock l{stoppedInfo.m};
stoppedInfo.cv.wait(l, [&i = stoppedInfo] { return i.event.has_value(); });
@@ -118,6 +129,7 @@ namespace armarx::navigation::client
void
Navigator::stopped(const StopEvent& e)
{
+ ARMARX_TRACE;
{
std::scoped_lock l{stoppedInfo.m};
stoppedInfo.event = e;
diff --git a/source/armarx/navigation/client/services/IceNavigator.cpp b/source/armarx/navigation/client/services/IceNavigator.cpp
index 6df5101d..a28e7046 100644
--- a/source/armarx/navigation/client/services/IceNavigator.cpp
+++ b/source/armarx/navigation/client/services/IceNavigator.cpp
@@ -43,6 +43,7 @@ namespace armarx::navigation::client
IceNavigator::createConfig(const client::NavigationStackConfig& config,
const std::string& configId)
{
+ ARMARX_TRACE;
this->configId = configId;
navigator->createConfig(config.toAron(), configId);
}
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index a53c8528..ed564421 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -314,7 +314,7 @@ namespace armarx::navigation::components
const Ice::Current&)
{
// TODO: Error handling.
-
+ ARMARX_TRACE;
ARMARX_INFO << "MoveTowards requested by caller '" << callerId << "'";
ARMARX_CHECK(navigators.count(callerId) > 0)
@@ -334,6 +334,7 @@ namespace armarx::navigation::components
void
components::Navigator::resume(const std::string& configId, const Ice::Current&)
{
+ ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
navigators.at(configId).resume();
@@ -342,6 +343,7 @@ namespace armarx::navigation::components
void
components::Navigator::stop(const std::string& configId, const Ice::Current&)
{
+ ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
navigators.at(configId).stop();
@@ -350,6 +352,7 @@ namespace armarx::navigation::components
void
components::Navigator::stopAll(const Ice::Current&)
{
+ ARMARX_TRACE;
for (auto& [_, navigator] : navigators)
{
navigator.stop();
@@ -359,6 +362,7 @@ namespace armarx::navigation::components
bool
components::Navigator::isPaused(const std::string& configId, const Ice::Current&)
{
+ ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
return navigators.at(configId).isPaused();
@@ -367,6 +371,7 @@ namespace armarx::navigation::components
bool
components::Navigator::isStopped(const std::string& configId, const Ice::Current&)
{
+ ARMARX_TRACE;
ARMARX_CHECK(navigators.count(configId) > 0)
<< "Navigator config for caller `" << configId << "` not registered!";
return navigators.at(configId).isStopped();
@@ -501,6 +506,7 @@ namespace armarx::navigation::components
void
components::Navigator::updateRobot()
{
+ ARMARX_TRACE;
synchronizeLocalClone(scene.robot);
auto robotDescription = virtualRobotReader.queryDescription("Armar6", armem::Time::now());
@@ -530,6 +536,7 @@ namespace armarx::navigation::components
server::Navigator*
components::Navigator::activeNavigator()
{
+ ARMARX_TRACE;
// We define the active navigator to be the one whose movement is enabled.
const auto isActive = [](auto& nameNavPair) -> bool
{
--
GitLab
From f5d5fba4d58705e91e548a4cc1e42fffa43dab9c Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 22 Dec 2021 20:25:38 +0100
Subject: [PATCH 03/16] navigator: minor
---
source/armarx/navigation/server/Navigator.cpp | 58 +++++++++++--------
1 file changed, 35 insertions(+), 23 deletions(-)
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index bb1a6296..5653dd7c 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -88,7 +88,7 @@ namespace armarx::navigation::server
case armarx::navigation::client::GlobalPlanningStrategy::Free:
{
ARMARX_VERBOSE << "Global planning from " << start.translation() << " to "
- << goal.translation();
+ << goal.translation();
ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
try
@@ -97,7 +97,8 @@ namespace armarx::navigation::server
if (globalPlan.has_value())
{
edge.attrib().trajectory = globalPlan->trajectory;
- ARMARX_VERBOSE << "Free path length: " << globalPlan->trajectory.length();
+ ARMARX_VERBOSE << "Free path length: "
+ << globalPlan->trajectory.length();
edge.attrib().cost() = globalPlan->trajectory.length();
}
else
@@ -109,7 +110,7 @@ namespace armarx::navigation::server
catch (...)
{
ARMARX_VERBOSE << "Global planning failed due to exception "
- << GetHandledExceptionString();
+ << GetHandledExceptionString();
edge.attrib().cost() = std::numeric_limits<float>::max();
}
@@ -162,7 +163,7 @@ namespace armarx::navigation::server
const core::Graph::ConstVertex& goalVertex)
{
ARMARX_VERBOSE << "Graph consists of " << graph.numVertices() << " vertices and "
- << graph.numEdges() << " edges.";
+ << graph.numEdges() << " edges.";
std::vector<core::Graph::VertexDescriptor> predecessors(graph.numVertices());
std::vector<int> d(num_vertices(graph));
@@ -189,7 +190,7 @@ namespace armarx::navigation::server
for (const auto& edge : graph.edges())
{
ARMARX_VERBOSE << edge.sourceObjectID() << " -> " << edge.targetObjectID() << ": "
- << edge.attrib().m_value;
+ << edge.attrib().m_value;
}
std::vector<core::Graph::EdgeDescriptor> edgesToBeRemoved;
@@ -210,15 +211,15 @@ namespace armarx::navigation::server
for (const auto& edge : graph.edges())
{
ARMARX_VERBOSE << edge.sourceObjectID() << " -> " << edge.targetObjectID() << ": "
- << edge.attrib().m_value;
+ << edge.attrib().m_value;
ARMARX_VERBOSE << "Edge: " << edge << "cost: " << edge.attrib().cost()
- << ", type: " << static_cast<int>(edge.attrib().strategy) << " , has traj "
- << edge.attrib().trajectory.has_value();
+ << ", type: " << static_cast<int>(edge.attrib().strategy)
+ << " , has traj " << edge.attrib().trajectory.has_value();
}
ARMARX_VERBOSE << "Searching shortest path from vertex with id `"
- << graph.vertex(start).objectID() << "` to vertex `"
- << graph.vertex(goalVertex.descriptor()).objectID() << "`";
+ << graph.vertex(start).objectID() << "` to vertex `"
+ << graph.vertex(goalVertex.descriptor()).objectID() << "`";
boost::dijkstra_shortest_paths(graph, start, params);
@@ -243,8 +244,9 @@ namespace armarx::navigation::server
// find edge between parent and currentVertex
auto outEdges = graph.vertex(parent).outEdges();
ARMARX_VERBOSE << "Parent has " << std::distance(outEdges.begin(), outEdges.end())
- << " out edges";
- ARMARX_CHECK(std::distance(outEdges.begin(), outEdges.end()) > 0); // not empty
+ << " out edges";
+
+ ARMARX_CHECK_GREATER(std::distance(outEdges.begin(), outEdges.end()), 0) << "Cannot reach another vertex from vertex `" << graph.vertex(parent).objectID(); // not empty
auto edgeIt = std::find_if(outEdges.begin(),
outEdges.end(),
@@ -361,18 +363,25 @@ namespace armarx::navigation::server
// resample event straight lines
ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
- const core::Trajectory segmentTraj({trajectoryPoints.back(), nextTrajPt});
- ARMARX_INFO << "Segment trajectory length: " << segmentTraj.length();
+ // const core::Trajectory segmentTraj({trajectoryPoints.back(), nextTrajPt});
+ // ARMARX_INFO << "Segment trajectory length: " << segmentTraj.length();
+
+ // const auto resampledTrajectory = segmentTraj.resample(500); // FIXME param
- const auto resampledTrajectory = segmentTraj.resample(500); // FIXME param
+ // ARMARX_INFO << "Resampled trajectory contains "
+ // << resampledTrajectory.points().size() << " points";
- ARMARX_INFO << "Resampled trajectory contains "
- << resampledTrajectory.points().size() << " points";
+ // this is the same pose as the goal of the previous segment but with a different velocity
+ // FIXME MUST set velocity here.
+ // trajectoryPoints.push_back(
+ // core::TrajectoryPoint{.waypoint = trajectoryPoints.back().waypoint,
+ // .velocity = std::numeric_limits<float>::max()});
ARMARX_TRACE;
- trajectoryPoints.insert(trajectoryPoints.end(),
- resampledTrajectory.points().begin(),
- resampledTrajectory.points().end());
+ // trajectoryPoints.insert(trajectoryPoints.end(),
+ // resampledTrajectory.points().begin(),
+ // resampledTrajectory.points().end());
+ trajectoryPoints.push_back(nextTrajPt);
break;
}
@@ -533,6 +542,8 @@ namespace armarx::navigation::server
ARMARX_VERBOSE << graph;
+ srv.introspector->onGlobalGraph(graph);
+
ARMARX_TRACE;
const auto shortestPath = findShortestPath(graph, startVertex.value(), goalVertex);
@@ -858,8 +869,9 @@ namespace armarx::navigation::server
const core::Rotation robot_R_world(config.scene->robot->getGlobalOrientation().inverse());
- ARMARX_VERBOSE << deactivateSpam(100) << "Robot orientation "
- << simox::math::mat3f_to_rpy(config.scene->robot->getGlobalOrientation()).z();
+ ARMARX_VERBOSE
+ << deactivateSpam(100) << "Robot orientation "
+ << simox::math::mat3f_to_rpy(config.scene->robot->getGlobalOrientation()).z();
core::Twist robotFrameVelocity;
@@ -868,7 +880,7 @@ namespace armarx::navigation::server
robotFrameVelocity.angular = twist.angular;
ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
- << robotFrameVelocity.linear;
+ << robotFrameVelocity.linear;
srv.executor->move(robotFrameVelocity);
}
--
GitLab
From 97bd65c143770adf9bd4a1050b7d427b1833a1bb Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 22 Dec 2021 20:26:20 +0100
Subject: [PATCH 04/16] introspector itf: adding method onGlobalGraph
---
.../introspection/ArvizIntrospector.cpp | 34 +++++++++++++++++++
.../server/introspection/ArvizIntrospector.h | 3 ++
.../introspection/IntrospectorInterface.h | 2 ++
.../server/introspection/MemoryIntrospector.h | 5 +++
4 files changed, 44 insertions(+)
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 21e26387..5607b29e 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -175,4 +175,38 @@ namespace armarx::navigation::server
return *this;
}
+ void
+ ArvizIntrospector::onGlobalGraph(const core::Graph& graph)
+ {
+ auto layer = arviz.layer("global_planning_graph");
+
+
+ for (const auto& edge : graph.edges())
+ {
+
+ const auto sourcePose = graph.vertex(edge.sourceDescriptor()).attrib().getPose();
+ const auto targetPose = graph.vertex(edge.targetDescriptor()).attrib().getPose();
+
+
+ const viz::Color color = [&]()
+ {
+ if (edge.attrib().cost() > 100'000) // "NaN check"
+ {
+ return viz::Color::red();
+ }
+
+ return viz::Color::green();
+ }();
+
+
+ layer.add(viz::Arrow(std::to_string(edge.sourceObjectID().t) + " -> " +
+ std::to_string(edge.targetObjectID().t))
+ .fromTo(sourcePose.translation(), targetPose.translation())
+ .color(color));
+ }
+
+ arviz.commit(layer);
+ }
+
+
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index cc67dccd..8d20cd8c 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -61,6 +61,9 @@ namespace armarx::navigation::server
void onGlobalShortestPath(const std::vector<core::Pose>& path) override;
+ void onGlobalGraph(const core::Graph& graph) override;
+
+
// // Non-API
ArvizIntrospector(ArvizIntrospector&& other) noexcept;
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 50b6ca85..316cb9ba 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -47,6 +47,8 @@ namespace armarx::navigation::server
virtual void onGlobalShortestPath(const std::vector<core::Pose>& path) = 0;
+ virtual void onGlobalGraph(const core::Graph& graph) = 0;
+
protected:
private:
};
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index 0e4a5354..2efb102a 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -51,6 +51,11 @@ namespace armarx::navigation::server
{
}
+ void
+ onGlobalGraph(const core::Graph& graph) override
+ {
+ }
+
protected:
private:
--
GitLab
From a83565e0f5c020127db84e6d360aacd66a2d11aa Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 22 Dec 2021 20:26:39 +0100
Subject: [PATCH 05/16] edge: no aron str
---
source/armarx/navigation/core/Graph.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/core/Graph.cpp b/source/armarx/navigation/core/Graph.cpp
index f1bc3781..ac2fc21e 100644
--- a/source/armarx/navigation/core/Graph.cpp
+++ b/source/armarx/navigation/core/Graph.cpp
@@ -81,7 +81,8 @@ namespace armarx::navigation::core
ss << " cost " << edge.attrib().cost()
<< ", type: " << static_cast<int>(edge.attrib().strategy) << " , has traj "
- << edge.attrib().trajectory.has_value() << ", aron: " << nlohmann::json(edge.attrib().aron);
+ << edge.attrib().trajectory.has_value();
+ // << ", aron: " << nlohmann::json(edge.attrib().aron);
return ss.str();
}
--
GitLab
From e3901eac25908f08de71504109ad54727174ebd9 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 22 Dec 2021 20:27:42 +0100
Subject: [PATCH 06/16] component plugin: properly setting subscriber to handle
memory events
---
.../navigation/client/ComponentPlugin.cpp | 31 +++++++++++++++++++
.../navigation/client/ComponentPlugin.h | 16 ++++++++++
2 files changed, 47 insertions(+)
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index 07cb43c0..0f35e89d 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -1,8 +1,16 @@
+#include <memory>
+
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include "RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h"
+#include "RobotAPI/libraries/armem/client/MemoryNameSystem.h"
+#include "RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h"
+#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
+
+#include "armarx/navigation/client/services/MemorySubscriber.h"
#include <armarx/navigation/client/ComponentPlugin.h>
#include <armarx/navigation/client/Navigator.h>
@@ -37,6 +45,12 @@ armarx::navigation::client::ComponentPlugin::preOnConnectComponent()
ARMARX_TRACE;
const std::string componentName = parent().getName();
+ if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
+ {
+ auto mns = parent<armarx::armem::client::plugins::ListeningPluginUser>().memoryNameSystem();
+ parent<armarx::navigation::client::ComponentPluginUser>().memoryNameSystem = &mns;
+ }
+
ARMARX_CHECK_NOT_NULL(navigatorPrx) << "Navigator proxy is null!";
iceNavigator.setNavigatorComponent(navigatorPrx);
}
@@ -66,4 +80,21 @@ armarx::navigation::client::ComponentPluginUser::configureNavigator(
plugin->configureNavigator(stackConfig, getName());
}
+void
+armarx::navigation::client::ComponentPluginUser::configureNavigator(
+ const client::NavigationStackConfig& stackConfig,
+ const std::string& configId)
+{
+ ARMARX_CHECK_NULL(navigatorServices.subscriber)
+ << "`configureNavigator` can only be called once atm.";
+
+ if (memoryNameSystem != nullptr)
+ {
+ memorySubscriber = std::make_unique<MemorySubscriber>(configId, *memoryNameSystem);
+ navigatorServices.subscriber = memorySubscriber.get();
+ }
+
+ iceNavigator.createConfig(stackConfig, configId);
+}
+
armarx::navigation::client::ComponentPluginUser::~ComponentPluginUser() = default;
diff --git a/source/armarx/navigation/client/ComponentPlugin.h b/source/armarx/navigation/client/ComponentPlugin.h
index ce6c5ad8..0ecfccdf 100644
--- a/source/armarx/navigation/client/ComponentPlugin.h
+++ b/source/armarx/navigation/client/ComponentPlugin.h
@@ -3,6 +3,7 @@
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
+#include "RobotAPI/libraries/armem/client/MemoryNameSystem.h"
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
@@ -62,7 +63,22 @@ namespace armarx::navigation::client
~ComponentPluginUser() override;
private:
+ IceNavigator iceNavigator;
+
+ armem::client::MemoryNameSystem* memoryNameSystem;
+ // MemorySubscriber memorySubscriber;
+ // SimpleEventHandler memorySubscriber; // FIXME
+ Navigator::InjectedServices navigatorServices{.navigator = &iceNavigator,
+ .subscriber = nullptr};
+
+
ComponentPlugin* plugin = nullptr;
+
+ std::unique_ptr<MemorySubscriber> memorySubscriber;
+
+
+ public:
+ Navigator navigator;
};
} // namespace armarx::navigation::client
--
GitLab
From 4bad88ef21ad19c8e0524a4d71e32a22392021af Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 22 Dec 2021 21:15:40 +0100
Subject: [PATCH 07/16] component plugin: properly registering event handler
---
.../navigation/client/ComponentPlugin.cpp | 57 ++++++++++---------
.../navigation/client/ComponentPlugin.h | 38 ++++---------
2 files changed, 41 insertions(+), 54 deletions(-)
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index 0f35e89d..628d090c 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -1,18 +1,19 @@
#include <memory>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include "RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h"
-#include "RobotAPI/libraries/armem/client/MemoryNameSystem.h"
-#include "RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h"
-#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
+#include <RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
-#include "armarx/navigation/client/services/MemorySubscriber.h"
+#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/ComponentPlugin.h>
#include <armarx/navigation/client/Navigator.h>
+#include <armarx/navigation/client/services/MemorySubscriber.h>
// ComponentPlugin
@@ -45,12 +46,6 @@ armarx::navigation::client::ComponentPlugin::preOnConnectComponent()
ARMARX_TRACE;
const std::string componentName = parent().getName();
- if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
- {
- auto mns = parent<armarx::armem::client::plugins::ListeningPluginUser>().memoryNameSystem();
- parent<armarx::navigation::client::ComponentPluginUser>().memoryNameSystem = &mns;
- }
-
ARMARX_CHECK_NOT_NULL(navigatorPrx) << "Navigator proxy is null!";
iceNavigator.setNavigatorComponent(navigatorPrx);
}
@@ -61,7 +56,29 @@ armarx::navigation::client::ComponentPlugin::configureNavigator(
const std::string& configId)
{
ARMARX_TRACE;
+
+ ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
+
+ eventHandler = [&]() -> std::unique_ptr<SimpleEventHandler>
+ {
+ if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
+ {
+ ARMARX_INFO << "Using memory event callbacks.";
+ auto memoryNameSystem =
+ parent<armarx::armem::client::plugins::ListeningPluginUser>().memoryNameSystem();
+ return std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
+ }
+
+ ARMARX_INFO << "Cannot use memory event callbacks as this component is not derived from "
+ "`armarx::armem::client::plugins::ListeningPluginUser`";
+
+ return std::make_unique<SimpleEventHandler>();
+ }();
+
iceNavigator.createConfig(stackConfig, configId);
+
+ navigator = std::make_unique<Navigator>(Navigator::InjectedServices{
+ .navigator = &iceNavigator, .subscriber = eventHandler.get()});
}
// ComponentPluginUser
@@ -80,21 +97,5 @@ armarx::navigation::client::ComponentPluginUser::configureNavigator(
plugin->configureNavigator(stackConfig, getName());
}
-void
-armarx::navigation::client::ComponentPluginUser::configureNavigator(
- const client::NavigationStackConfig& stackConfig,
- const std::string& configId)
-{
- ARMARX_CHECK_NULL(navigatorServices.subscriber)
- << "`configureNavigator` can only be called once atm.";
-
- if (memoryNameSystem != nullptr)
- {
- memorySubscriber = std::make_unique<MemorySubscriber>(configId, *memoryNameSystem);
- navigatorServices.subscriber = memorySubscriber.get();
- }
-
- iceNavigator.createConfig(stackConfig, configId);
-}
armarx::navigation::client::ComponentPluginUser::~ComponentPluginUser() = default;
diff --git a/source/armarx/navigation/client/ComponentPlugin.h b/source/armarx/navigation/client/ComponentPlugin.h
index 0ecfccdf..0f63131a 100644
--- a/source/armarx/navigation/client/ComponentPlugin.h
+++ b/source/armarx/navigation/client/ComponentPlugin.h
@@ -3,25 +3,24 @@
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
-#include "RobotAPI/libraries/armem/client/MemoryNameSystem.h"
+#include <armarx/navigation/client/ComponentPlugin.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/services/IceNavigator.h>
-#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
namespace armarx::navigation::client
{
+ class Navigator;
+
class ComponentPlugin : public armarx::ComponentPlugin
{
-
public:
ComponentPlugin(ManagedIceObject& parent, const std::string& prefix) :
- armarx::ComponentPlugin(parent, prefix), navigator(navigatorServices){};
-// using armarx::ComponentPlugin::ComponentPlugin;
+ armarx::ComponentPlugin(parent, prefix){};
~ComponentPlugin() override;
@@ -39,13 +38,11 @@ namespace armarx::navigation::client
client::NavigatorInterfacePrx navigatorPrx;
IceNavigator iceNavigator;
- // MemorySubscriber memorySubscriber;
- SimpleEventHandler memorySubscriber; // FIXME
- Navigator::InjectedServices navigatorServices{.navigator = &iceNavigator,
- .subscriber = &memorySubscriber};
+
+ std::unique_ptr<SimpleEventHandler> eventHandler;
public:
- Navigator navigator;
+ std::unique_ptr<Navigator> navigator;
};
@@ -56,29 +53,18 @@ namespace armarx::navigation::client
ComponentPluginUser();
void configureNavigator(const client::NavigationStackConfig& stackConfig);
- Navigator& getNavigator() {
- return plugin->navigator;
+
+ Navigator&
+ getNavigator()
+ {
+ return *plugin->navigator;
}
// Non-API
~ComponentPluginUser() override;
private:
- IceNavigator iceNavigator;
-
- armem::client::MemoryNameSystem* memoryNameSystem;
- // MemorySubscriber memorySubscriber;
- // SimpleEventHandler memorySubscriber; // FIXME
- Navigator::InjectedServices navigatorServices{.navigator = &iceNavigator,
- .subscriber = nullptr};
-
-
ComponentPlugin* plugin = nullptr;
- std::unique_ptr<MemorySubscriber> memorySubscriber;
-
-
- public:
- Navigator navigator;
};
} // namespace armarx::navigation::client
--
GitLab
From 19085dbaee9839611223b907a1e906fa8c3a43ee Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 22 Dec 2021 21:19:04 +0100
Subject: [PATCH 08/16] cleanup
---
source/armarx/navigation/client/ComponentPlugin.cpp | 13 ++++++++++---
source/armarx/navigation/client/ComponentPlugin.h | 7 ++-----
2 files changed, 12 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index 628d090c..4cfeeb8b 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -10,10 +10,10 @@
#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
-#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/client/ComponentPlugin.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
+#include <armarx/navigation/client/services/SimpleEventHandler.h>
// ComponentPlugin
@@ -77,8 +77,8 @@ armarx::navigation::client::ComponentPlugin::configureNavigator(
iceNavigator.createConfig(stackConfig, configId);
- navigator = std::make_unique<Navigator>(Navigator::InjectedServices{
- .navigator = &iceNavigator, .subscriber = eventHandler.get()});
+ navigator = std::make_unique<Navigator>(
+ Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
}
// ComponentPluginUser
@@ -97,5 +97,12 @@ armarx::navigation::client::ComponentPluginUser::configureNavigator(
plugin->configureNavigator(stackConfig, getName());
}
+armarx::navigation::client::Navigator&
+armarx::navigation::client::ComponentPluginUser::getNavigator()
+{
+ ARMARX_CHECK_NOT_NULL(plugin->navigator)
+ << "You need to call `configureNavigator()` before you can access the navigator!";
+ return *plugin->navigator;
+}
armarx::navigation::client::ComponentPluginUser::~ComponentPluginUser() = default;
diff --git a/source/armarx/navigation/client/ComponentPlugin.h b/source/armarx/navigation/client/ComponentPlugin.h
index 0f63131a..d5a670a8 100644
--- a/source/armarx/navigation/client/ComponentPlugin.h
+++ b/source/armarx/navigation/client/ComponentPlugin.h
@@ -54,11 +54,8 @@ namespace armarx::navigation::client
void configureNavigator(const client::NavigationStackConfig& stackConfig);
- Navigator&
- getNavigator()
- {
- return *plugin->navigator;
- }
+ Navigator& getNavigator();
+
// Non-API
~ComponentPluginUser() override;
--
GitLab
From 9befedc318d89d77371261b4d7c921f3d77ec86f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 22 Dec 2021 22:49:15 +0100
Subject: [PATCH 09/16] using newly defined collision model
---
.../algorithms/astar/AStarPlanner.cpp | 4 +-
.../navigation/global_planning/AStar.cpp | 85 ++++++++++---------
2 files changed, 46 insertions(+), 43 deletions(-)
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 0386c314..85ace90b 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -124,8 +124,8 @@ namespace armarx::navigation::algorithm::astar
ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
ARMARX_CHECK_NOT_NULL(obstacles);
- const core::Pose globalPose = Eigen::Translation3f(conv::to3D(n->position)) *
- Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
+ const core::Pose globalPose(Eigen::Translation3f(conv::to3D(n->position))); //*
+ //Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
robotCollisionModel->setGlobalPose(globalPose.matrix());
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 8cc0acb3..bf4b3561 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -73,58 +73,60 @@ namespace armarx::navigation::glob_plan
{
}
- auto
- robotCollisionModel()
- {
+ // auto
+ // robotCollisionModel()
+ // {
- constexpr float platformHeight = 600;
- constexpr float bodyHeight = 1400;
- constexpr float fullHeight = platformHeight + bodyHeight;
+ // constexpr float platformHeight = 600;
+ // constexpr float bodyHeight = 1400;
+ // constexpr float fullHeight = platformHeight + bodyHeight;
- constexpr float platformRadius = 500.F;
- constexpr float bodyRadius = 300.F;
- constexpr float simpleRadius = std::max(platformRadius, bodyRadius);
+ // constexpr float platformRadius = 600.F;
+ // constexpr float bodyRadius = 300.F;
+ // constexpr float simpleRadius = std::max(platformRadius, bodyRadius);
- // combined model made out of two stacked cylinders.
- [[maybe_unused]]
- const auto createCombinedVisuModel = []()
- {
- VirtualRobot::CoinVisualizationFactory factory;
+ // // combined model made out of two stacked cylinders.
+ // [[maybe_unused]] const auto createCombinedVisuModel = []()
+ // {
+ // VirtualRobot::CoinVisualizationFactory factory;
- auto cylinderPlatform = factory.createCylinder(platformRadius, platformHeight);
- factory.applyDisplacement(
- cylinderPlatform,
- core::Pose(Eigen::Translation3f{Eigen::Vector3f{0, 0, platformHeight / 2}})
- .matrix());
+ // auto cylinderPlatform = factory.createCylinder(platformRadius, platformHeight);
+ // factory.applyDisplacement(
+ // cylinderPlatform,
+ // core::Pose(Eigen::Translation3f{platformHeight / 2 * Eigen::Vector3f::UnitY()})
+ // .matrix());
- auto cylinderBody = factory.createCylinder(bodyRadius, bodyHeight);
- factory.applyDisplacement(cylinderPlatform,
- core::Pose(Eigen::Translation3f{Eigen::Vector3f{
- 0, 0, bodyHeight / 2 + platformHeight}})
- .matrix());
+ // auto cylinderBody = factory.createCylinder(bodyRadius, bodyHeight);
+ // factory.applyDisplacement(
+ // cylinderPlatform,
+ // core::Pose(Eigen::Translation3f{(bodyHeight / 2 + platformHeight) *
+ // Eigen::Vector3f::UnitY()})
+ // .matrix());
- return factory.createUnitedVisualization({cylinderPlatform, cylinderBody});
- };
- // simple model made of one cylinder only
- const auto createSimpleVisuModel = []()
- {
- VirtualRobot::CoinVisualizationFactory factory;
+ // return factory.createUnitedVisualization({cylinderPlatform, cylinderBody});
+ // };
- auto cylinderPlatform = factory.createCylinder(simpleRadius, fullHeight);
- factory.applyDisplacement(
- cylinderPlatform,
- core::Pose(Eigen::Translation3f{Eigen::Vector3f{0, 0, fullHeight / 2}}).matrix());
+ // // simple model made of one cylinder only
+ // const auto createSimpleVisuModel = []()
+ // {
+ // VirtualRobot::CoinVisualizationFactory factory;
- return cylinderPlatform;
- };
+ // auto cylinderPlatform = factory.createCylinder(simpleRadius, fullHeight);
+ // factory.applyDisplacement(
+ // cylinderPlatform,
+ // core::Pose(Eigen::Translation3f{fullHeight / 2 * Eigen::Vector3f::UnitY()})
+ // .matrix());
- const auto visuModel = createSimpleVisuModel();
+ // return cylinderPlatform;
+ // };
- VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(visuModel));
+ // const auto visuModel = createSimpleVisuModel();
- return collisionModel;
- }
+ // VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(visuModel));
+
+ // return collisionModel;
+ // }
std::vector<Eigen::Vector2f>
AStar::postProcessPath(const std::vector<Eigen::Vector2f>& path)
@@ -160,7 +162,8 @@ namespace armarx::navigation::glob_plan
// planner.setRobotColModel("Platform-colmodel");
// planner.setRobotColModel("Platform-colmodel");
- planner.setRobotCollisionModel(robotCollisionModel());
+ //planner.setRobotCollisionModel(robotCollisionModel());
+ planner.setRobotColModel("PlatformNavigationColModel");
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
--
GitLab
From 8d17adbb1fcda8752e4ca57755e75202c0a7372e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 3 Jan 2022 09:44:21 +0100
Subject: [PATCH 10/16] clang tidy update
---
.clang-tidy | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/.clang-tidy b/.clang-tidy
index fc2f7fe9..6e80b902 100644
--- a/.clang-tidy
+++ b/.clang-tidy
@@ -67,7 +67,7 @@ CheckOptions:
- key: readability-identifier-naming.ConstantMemberPrefix
value: ''
- key: readability-identifier-naming.ConstantMemberCase
- value: UPPER_CASE
+ value: camelBack
- key: readability-identifier-naming.ConstantParameterPrefix
value: ''
- key: readability-identifier-naming.ConstantParameterCase
--
GitLab
From 925ad9c71d27765e4d5f4c0ea2c074de5a416c5e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 3 Jan 2022 09:45:40 +0100
Subject: [PATCH 11/16] using proper robot collision model
---
.../algorithms/astar/AStarPlanner.cpp | 2 +-
.../navigation/global_planning/AStar.cpp | 3 +-
.../test/orientation_optimization_test.cpp | 128 ++++++++++++++++++
3 files changed, 131 insertions(+), 2 deletions(-)
create mode 100644 source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 85ace90b..0213bd31 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -186,6 +186,7 @@ namespace armarx::navigation::algorithm::astar
// << "No collision model for robot node " << robotColModelName;
// robotCollisionModel = robot->getRobotNode(robotColModelName)->getCollisionModel()->clone();
+ ARMARX_CHECK_NOT_NULL(robotCollisionModel);
robotCollisionModel->setGlobalPose(robot->getGlobalPose());
// static auto cylinder = VirtualRobot::ObjectIO::loadObstacle("/home/fabi/cylinder.xml");
@@ -194,7 +195,6 @@ namespace armarx::navigation::algorithm::astar
// robotCollisionModel = cylinder->getCollisionModel();
- ARMARX_CHECK_NOT_NULL(robotCollisionModel);
createUniformGrid();
ARMARX_DEBUG << "Setting start node for position " << start;
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index bf4b3561..9d6f0033 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -6,6 +6,7 @@
#include <Eigen/Geometry>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
@@ -163,7 +164,7 @@ namespace armarx::navigation::glob_plan
// planner.setRobotColModel("Platform-colmodel");
//planner.setRobotCollisionModel(robotCollisionModel());
- planner.setRobotColModel("PlatformNavigationColModel");
+ planner.setRobotCollisionModel(scene.robot->getRobotNode("Platform-navigation-colmodel")->getCollisionModel());
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
diff --git a/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp b/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
new file mode 100644
index 00000000..c584146c
--- /dev/null
+++ b/source/armarx/navigation/global_planning/test/orientation_optimization_test.cpp
@@ -0,0 +1,128 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package Navigation::ArmarXObjects::global_planning
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <cstddef>
+
+#include <boost/test/tools/old/interface.hpp>
+
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::global_planning
+
+#define ARMARX_BOOST_TEST
+
+#include <cmath>
+#include <iostream>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "armarx/navigation/core/basic_types.h"
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/global_planning/optimization/OrientationOptimizer.h>
+
+BOOST_AUTO_TEST_CASE(testOrientationOptimizationRotatedGoal)
+{
+ namespace nav = armarx::navigation;
+ namespace gp = armarx::navigation::glob_plan;
+
+ nav::core::Path path;
+ path.emplace_back(nav::core::Pose::Identity());
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 500});
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 1000});
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 1500});
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 2000} *
+ Eigen::AngleAxis(M_PI_2f32, Eigen::Vector3f::UnitZ()));
+
+ const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+
+ gp::OrientationOptimizer::Params params{};
+
+
+ gp::OrientationOptimizer optimizer(trajectory, params);
+
+ const gp::OrientationOptimizer::OptimizationResult result = optimizer.optimize();
+
+ // # of points should be unchanged
+ BOOST_REQUIRE_EQUAL(result.trajectory.poses().size(), trajectory.poses().size());
+
+ // avoid an underflow in the for loop below
+ BOOST_REQUIRE_GE(trajectory.poses().size(), 2);
+
+ // loop over intermediate points
+ for (std::size_t i = 0; i < (result.trajectory.poses().size() - 2); i++)
+ {
+ const auto rpy = simox::math::mat3f_to_rpy(result.trajectory.poses().at(i).linear());
+ const auto yaw = rpy.z();
+
+ // The orientation should be changed compared to the unoptimized version.
+ // As the last pose is rotated by 90°, the intermediate poses should at least be
+ // changed gentlty into this direction.
+ BOOST_CHECK_GT(yaw, 0.1);
+ }
+}
+
+
+BOOST_AUTO_TEST_CASE(testOrientationOptimizationCircular)
+{
+ namespace nav = armarx::navigation;
+ namespace gp = armarx::navigation::glob_plan;
+
+ nav::core::Path path;
+ path.emplace_back(nav::core::Pose::Identity());
+ for (int i = 0; i < 20; i++)
+ {
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 500 * i} *
+ Eigen::AngleAxis(M_PI_2f32 * i, Eigen::Vector3f::UnitZ()));
+ }
+
+ path.emplace_back(Eigen::Translation3f{Eigen::Vector3f::UnitX() * 500 * 20});
+
+
+ const auto trajectory = nav::core::Trajectory::FromPath(path, 100);
+
+ // Here we don't care about the orientatation prior.
+ // The orientation of the intermediate points should converge to 0 (same direction as start and goal)
+ gp::OrientationOptimizer::Params params{.priorWeight = 0.F};
+
+
+ gp::OrientationOptimizer optimizer(trajectory, params);
+
+ const gp::OrientationOptimizer::OptimizationResult result = optimizer.optimize();
+
+ // # of points should be unchanged
+ BOOST_REQUIRE_EQUAL(result.trajectory.poses().size(), trajectory.poses().size());
+
+ // avoid an underflow in the for loop below
+ BOOST_REQUIRE_GE(trajectory.poses().size(), 2);
+
+ // loop over intermediate points
+ for (std::size_t i = 0; i < (result.trajectory.poses().size() - 2); i++)
+ {
+ const auto rpy = simox::math::mat3f_to_rpy(result.trajectory.poses().at(i).linear());
+ const auto yaw = rpy.z();
+
+ // The orientation should be changed compared to the unoptimized version.
+ // As the last pose is oriented the same way as the start, the intermediate poses should
+ // have the same orientation. => yaw ~= 0
+ BOOST_CHECK_LT(std::abs(yaw), 0.01);
+ }
+}
--
GitLab
From c56518fe8e244a52eb081117fcf8b5c819218003 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 3 Jan 2022 09:47:51 +0100
Subject: [PATCH 12/16] tests
---
.../navigation/global_planning/CMakeLists.txt | 8 ++++
.../test/global_planningTest.cpp | 38 -------------------
2 files changed, 8 insertions(+), 38 deletions(-)
delete mode 100644 source/armarx/navigation/global_planning/test/global_planningTest.cpp
diff --git a/source/armarx/navigation/global_planning/CMakeLists.txt b/source/armarx/navigation/global_planning/CMakeLists.txt
index 87d9bd68..4de838c4 100644
--- a/source/armarx/navigation/global_planning/CMakeLists.txt
+++ b/source/armarx/navigation/global_planning/CMakeLists.txt
@@ -30,3 +30,11 @@ armarx_add_library(global_planning
./aron_conversions.h
./optimization/OrientationOptimizer.h
)
+
+
+armarx_add_test(orientation_optimization_test
+ TEST_FILES
+ test/orientation_optimization_test.cpp
+ DEPENDENCIES
+ armarx_navigation::global_planning
+)
diff --git a/source/armarx/navigation/global_planning/test/global_planningTest.cpp b/source/armarx/navigation/global_planning/test/global_planningTest.cpp
deleted file mode 100644
index 2518dcfd..00000000
--- a/source/armarx/navigation/global_planning/test/global_planningTest.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-/**
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package Navigation::ArmarXObjects::global_planning
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::global_planning
-
-#define ARMARX_BOOST_TEST
-
-#include "../global_planning.h"
-
-#include <iostream>
-
-#include <armarx/navigation/Test.h>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
-
- BOOST_CHECK_EQUAL(true, true);
-}
--
GitLab
From c975fb5eff94f7e2d69fa2f014be5179ed5c80fa Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 3 Jan 2022 09:48:29 +0100
Subject: [PATCH 13/16] periodic diff
---
.../optimization/OrientationOptimizer.cpp | 48 +++++++++++--------
1 file changed, 28 insertions(+), 20 deletions(-)
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 8cc3ec81..4b0f24a3 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -1,5 +1,7 @@
#include "OrientationOptimizer.h"
+#include <cmath>
+
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/drop.hpp>
#include <range/v3/view/drop_last.hpp>
@@ -8,9 +10,11 @@
#include <ceres/ceres.h>
#include <ceres/cost_function.h>
+#include <ceres/jet.h>
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/periodic/periodic_diff.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -29,16 +33,24 @@ namespace armarx::navigation::glob_plan
// problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1);
- struct CostFunctor
+ inline auto
+ periodicDiff(auto a, auto b)
{
- template <typename T>
- bool
- operator()(const T* const x, T* residual) const
- {
- residual[0] = 10.0 - x[0];
- return true;
- }
- };
+ // angle to corresponding vector
+ const auto a1 = ceres::cos(a);
+ const auto a2 = ceres::sin(a);
+
+ // angle to corresponding vector
+ const auto b1 = ceres::cos(b);
+ const auto b2 = ceres::sin(b);
+
+ // cosine similarity
+ const auto cosSim = a1 * b1 + a2 * b2;
+
+ const auto angleDiff = ceres::acos(cosSim);
+
+ return angleDiff;
+ }
struct SmoothOrientationCostFunctor
{
@@ -54,9 +66,8 @@ namespace armarx::navigation::glob_plan
const T* const yawNext,
T* residual) const
{
- // TODO periodic
- residual[0] = weight * (yawPrev[0] - yaw[0]);
- residual[1] = weight * (yawNext[0] - yaw[0]);
+ residual[0] = weight * periodicDiff(yawPrev[0], yaw[0]);
+ residual[1] = weight * periodicDiff(yawNext[0], yaw[0]);
return true;
}
@@ -76,9 +87,8 @@ namespace armarx::navigation::glob_plan
bool
operator()(const T* const yaw, const T* const yawNext, T* residual) const
{
- // TODO periodic
- residual[0] = weight * (yawPre - yaw[0]);
- residual[1] = weight * (yawNext[0] - yaw[0]);
+ residual[0] = weight * periodicDiff(yawPre, yaw[0]);
+ residual[1] = weight * periodicDiff(yawNext[0], yaw[0]);
return true;
}
@@ -99,9 +109,8 @@ namespace armarx::navigation::glob_plan
bool
operator()(const T* const yawPre, const T* const yaw, T* residual) const
{
- // TODO periodic
- residual[0] = weight * (yawPre[0] - yaw[0]);
- residual[1] = weight * (yawNext - yaw[0]);
+ residual[0] = weight * periodicDiff(yawPre[0], yaw[0]);
+ residual[1] = weight * periodicDiff(yawNext, yaw[0]);
return true;
}
@@ -122,8 +131,7 @@ namespace armarx::navigation::glob_plan
bool
operator()(const T* const x, T* residual) const
{
- // TODO periodic
- residual[0] = weight * (prior - x[0]);
+ residual[0] = weight * periodicDiff(prior, x[0]);
return true;
}
--
GitLab
From 27e8a950fd28b6eac37c221f8486962ff5e379e1 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 3 Jan 2022 09:48:46 +0100
Subject: [PATCH 14/16] checking if weights are greater than zero: only then,
the edges will be added
---
.../optimization/OrientationOptimizer.cpp | 47 ++++++++++++-------
1 file changed, 31 insertions(+), 16 deletions(-)
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 4b0f24a3..00717c3a 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -171,31 +171,43 @@ namespace armarx::navigation::glob_plan
// ceres::AngleLocalParameterization::Create();
// prior for all waypoints
- for (size_t i = 1; i < (orientations.size() - 1); i++)
+ if (params.priorWeight > 0.F)
{
- ceres::CostFunction* priorCostFunction =
- new ceres::AutoDiffCostFunction<OrientationPriorCostFunctor, 1, 1>(
- new OrientationPriorCostFunctor(orientations.at(i), params.priorWeight));
-
- problem.AddResidualBlock(priorCostFunction, nullptr, &orientations.at(i));
+ for (size_t i = 1; i < (orientations.size() - 1); i++)
+ {
+ ceres::CostFunction* priorCostFunction =
+ new ceres::AutoDiffCostFunction<OrientationPriorCostFunctor, 1, 1>(
+ new OrientationPriorCostFunctor(orientations.at(i), params.priorWeight));
+
+ problem.AddResidualBlock(priorCostFunction, nullptr, &orientations.at(i));
+ }
}
+
// smooth waypoint orientation, no start and goal nodes involved!
- for (size_t i = 2; i < (orientations.size() - 2); i++)
+ if (params.smoothnessWeight > 0.F)
{
- ceres::CostFunction* smoothCostFunction =
- new ceres::AutoDiffCostFunction<SmoothOrientationCostFunctor, 2, 1, 1, 1>(
- new SmoothOrientationCostFunctor(params.smoothnessWeight));
-
- problem.AddResidualBlock(smoothCostFunction,
- nullptr,
- &orientations.at(i - 1),
- &orientations.at(i),
- &orientations.at(i + 1));
+ ARMARX_VERBOSE << "Enabled SmoothOrientationCost";
+ for (size_t i = 2; i < (orientations.size() - 2); i++)
+ {
+ ceres::CostFunction* smoothCostFunction =
+ new ceres::AutoDiffCostFunction<SmoothOrientationCostFunctor, 2, 1, 1, 1>(
+ new SmoothOrientationCostFunctor(params.smoothnessWeight));
+
+ problem.AddResidualBlock(smoothCostFunction,
+ nullptr,
+ &orientations.at(i - 1),
+ &orientations.at(i),
+ &orientations.at(i + 1));
+ }
}
+
// smooth waypoint orientation, involving start
+ if (params.smoothnessWeightStartGoal > 0.F)
{
+ ARMARX_VERBOSE << "Enabled SmoothOrientationFixedPreCost";
+
ceres::CostFunction* smoothCostFunction =
new ceres::AutoDiffCostFunction<SmoothOrientationFixedPreCostFunctor, 2, 1, 1>(
new SmoothOrientationFixedPreCostFunctor(orientations.front(),
@@ -206,7 +218,10 @@ namespace armarx::navigation::glob_plan
}
// smooth waypoint orientation, involving goal
+ if (params.smoothnessWeightStartGoal > 0.F)
{
+ ARMARX_VERBOSE << "Enabled SmoothOrientationFixedNextCost";
+
ceres::CostFunction* smoothCostFunction =
new ceres::AutoDiffCostFunction<SmoothOrientationFixedNextCostFunctor, 2, 1, 1>(
new SmoothOrientationFixedNextCostFunctor(orientations.back(),
--
GitLab
From c6b3679d6bb969157b7454367c18e789984ea010 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 3 Jan 2022 09:49:13 +0100
Subject: [PATCH 15/16] fine tuned orientation optimization
---
.../optimization/OrientationOptimizer.cpp | 13 ++++++++++++-
1 file changed, 12 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index 00717c3a..eb6aeee7 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -238,12 +238,23 @@ namespace armarx::navigation::glob_plan
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
+ options.max_num_iterations = 100;
+ options.function_tolerance = 0.01;
+
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
- std::cout << summary.BriefReport() << "\n";
+ std::cout << summary.FullReport() << "\n";
+ // std::cout << summary.BriefReport() << "\n";
ARMARX_INFO << "orientations after: " << orientations;
+ ARMARX_INFO << "Optimization: " << summary.iterations.size() << " iterations";
+
+ if(not summary.IsSolutionUsable())
+ {
+ ARMARX_ERROR << "Orientation optimization failed!";
+ // TODO write to file
+ }
const auto applyOrientation = [](const auto& p) -> core::TrajectoryPoint
{
--
GitLab
From 8c9f60cfcf2023217b85edba5eaafbf4bbfbf949 Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Tue, 4 Jan 2022 16:35:24 +0100
Subject: [PATCH 16/16] Add configureNavigator overwrite in case the user has
access to MemoryNameSystem
---
.../navigation/client/ComponentPlugin.cpp | 17 +++++++++++++++++
.../armarx/navigation/client/ComponentPlugin.h | 7 +++++++
2 files changed, 24 insertions(+)
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index 4cfeeb8b..6266c680 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -80,6 +80,23 @@ armarx::navigation::client::ComponentPlugin::configureNavigator(
navigator = std::make_unique<Navigator>(
Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
}
+void
+armarx::navigation::client::ComponentPlugin::configureNavigator(
+ const armarx::navigation::client::NavigationStackConfig& stackConfig,
+ const std::string& configId,
+ armarx::armem::client::MemoryNameSystem& memoryNameSystem)
+{
+ ARMARX_TRACE;
+
+ ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
+
+ eventHandler = std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
+
+ iceNavigator.createConfig(stackConfig, configId);
+
+ navigator = std::make_unique<Navigator>(
+ Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
+}
// ComponentPluginUser
diff --git a/source/armarx/navigation/client/ComponentPlugin.h b/source/armarx/navigation/client/ComponentPlugin.h
index d5a670a8..38ab565f 100644
--- a/source/armarx/navigation/client/ComponentPlugin.h
+++ b/source/armarx/navigation/client/ComponentPlugin.h
@@ -10,6 +10,10 @@
#include <armarx/navigation/client/services/IceNavigator.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
+namespace armarx::armem::client
+{
+ class MemoryNameSystem;
+}
namespace armarx::navigation::client
{
@@ -33,6 +37,9 @@ namespace armarx::navigation::client
void configureNavigator(const client::NavigationStackConfig& stackConfig,
const std::string& configId);
+ void configureNavigator(const client::NavigationStackConfig& stackConfig,
+ const std::string& configId, armarx::armem::client::MemoryNameSystem& memoryNameSystem);
+
private:
static constexpr const char* PROPERTY_NAME = "nav.NavigatorName";
--
GitLab