From 15826b17cabb1551c14899023636c9df0334d134 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 19 Aug 2022 19:31:49 +0200
Subject: [PATCH 1/7] first draft: update navigator
---
.../components/example_client/Component.cpp | 90 +++++++++-
.../components/example_client/Component.h | 14 ++
...ulatedObjectLocalizerDynamicSimulation.cfg | 8 -
.../HandUnitDynamicSimulationApp.LeftHand.cfg | 17 ++
...HandUnitDynamicSimulationApp.RightHand.cfg | 17 ++
.../config/ObjectMemory.cfg | 26 +--
.../config/RobotUnitSimulationApp.cfg | 2 +-
.../SelfLocalizationDynamicSimulationApp.cfg | 39 +++--
.../config/SimulatorViewerApp.cfg | 74 ++-------
.../config/VisionMemory.cfg | 26 +--
.../config/ObjectMemory.cfg | 97 ++++++++++-
.../config/VisionMemory.cfg | 97 ++++++++++-
.../config/example_client.cfg | 36 ----
.../config/navigation_memory.cfg | 22 ++-
.../PlatformNavigation/config/navigator.cfg | 51 ++++--
.../navigation/client/ComponentPlugin.cpp | 150 ++++++++---------
source/armarx/navigation/client/Navigator.cpp | 7 +
source/armarx/navigation/client/Navigator.h | 2 +
.../client/ice/NavigatorInterface.ice | 2 +
.../client/services/IceNavigator.cpp | 12 ++
.../navigation/client/services/IceNavigator.h | 5 +
.../components/navigator/CMakeLists.txt | 11 +-
.../components/navigator/Component.cpp | 84 +++++++++-
.../components/navigator/Component.h | 11 +-
.../navigation/core/NavigatorInterface.h | 6 +
source/armarx/navigation/server/Navigator.cpp | 155 ++++++++++++++++--
source/armarx/navigation/server/Navigator.h | 13 ++
27 files changed, 773 insertions(+), 301 deletions(-)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index afbf7503..7b48c323 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -36,10 +36,11 @@
#include <ArmarXCore/core/time/forward_declarations.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-#include <armarx/navigation/client/types.h>
-#include <armarx/navigation/global_planning/Point2Point.h>
+#include "armarx/navigation/core/types.h"
#include <armarx/navigation/client/PathBuilder.h>
+#include <armarx/navigation/client/types.h>
#include <armarx/navigation/global_planning/AStar.h>
+#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
@@ -68,7 +69,7 @@ namespace armarx::navigation::components::example_client
void
Component::onConnectComponent()
{
- task = new armarx::RunningTask<Component>(this, &Component::exampleNavigationPointToPoint);
+ task = new armarx::RunningTask<Component>(this, &Component::run);
task->start();
}
@@ -94,6 +95,74 @@ namespace armarx::navigation::components::example_client
return "ExampleClient";
}
+ void
+ Component::run()
+ {
+ switch (properties.mode)
+ {
+
+ case Mode::Standard:
+ ARMARX_IMPORTANT << "Running `Mode::Standard`";
+ exampleNavigation();
+ break;
+ case Mode::Complex:
+ ARMARX_IMPORTANT << "Running `Mode::Complex`";
+ exampleNavigationComplex();
+ break;
+ case Mode::PointToPoint:
+ ARMARX_IMPORTANT << "Running `Mode::PointToPoint`";
+ exampleNavigationPointToPoint();
+ break;
+ case Mode::UpdateNavigator:
+ ARMARX_IMPORTANT << "Running `Mode::UpdateNavigator`";
+ exampleNavigationUpdateNavigator();
+ break;
+ }
+ }
+
+ void
+ Component::exampleNavigationUpdateNavigator()
+ {
+ // Import relevant namespaces.
+ using namespace armarx::navigation;
+
+ ARMARX_INFO << "Configuring navigator";
+
+ // Create an example configuration valid for the following move* calls.
+ configureNavigator(
+ client::NavigationStackConfig()
+ .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
+ .globalPlanner(global_planning::AStarParams())
+ .trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams()));
+
+ // Example of registering a lambda as callback.
+ getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached!"; });
+
+ // Start moving to goal position using above config.
+
+ ARMARX_INFO << "Moving to goal pose";
+ core::Pose goal = core::Pose::Identity();
+ goal.translation() << 2000, 1000, 0;
+ getNavigator().moveTo({goal}, core::NavigationFrame::Absolute);
+ Clock::WaitFor(Duration::Seconds(1.0));
+
+
+ // after short period of time (before goal is reached), change goal
+ const std::vector<core::Pose> goals{
+ core::Pose(Eigen::Translation3f{1000, 2000, 0}),
+ core::Pose(Eigen::Translation3f{3000, 0, 0}),
+ core::Pose(Eigen::Translation3f{5000, 1000, 0}),
+ };
+
+ while (true)
+ {
+ for (const auto& nextGoal : goals)
+ {
+ Clock::WaitFor(Duration::Seconds(1.0));
+ getNavigator().update({nextGoal}, core::NavigationFrame::Absolute);
+ }
+ }
+ }
void
Component::exampleNavigation()
@@ -108,7 +177,8 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::AStarParams())
- .trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
+ .trajectoryController(
+ traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
// Example of registering a lambda as callback.
getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
@@ -207,7 +277,8 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::AStarParams())
- .trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
+ .trajectoryController(
+ traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
// Example of registering a lambda as callback.
getNavigator().onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
@@ -283,7 +354,8 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::Point2PointParams())
- .trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
+ .trajectoryController(
+ traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
Clock::WaitFor(Duration::Seconds(1));
@@ -366,6 +438,12 @@ namespace armarx::navigation::components::example_client
"relativeMovement",
"The distance between two target poses [mm]");
+ def->optional(properties.mode, "mode", "Which example to run")
+ .map({{"standard", Mode::Standard},
+ {"complex", Mode::Complex},
+ {"point_to_point", Mode::PointToPoint},
+ {"update_navigator", Mode::UpdateNavigator}});
+
return def;
}
diff --git a/examples/components/example_client/Component.h b/examples/components/example_client/Component.h
index 295d1964..2e622a35 100644
--- a/examples/components/example_client/Component.h
+++ b/examples/components/example_client/Component.h
@@ -35,6 +35,14 @@
namespace armarx::navigation::components::example_client
{
+ enum class Mode
+ {
+ Standard,
+ Complex,
+ PointToPoint,
+ UpdateNavigator
+ };
+
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup armarx_navigation-Components
@@ -75,6 +83,8 @@ namespace armarx::navigation::components::example_client
std::string getDefaultName() const override;
+ void run();
+
void exampleNavigation();
void exampleNavigationComplex();
@@ -91,12 +101,16 @@ namespace armarx::navigation::components::example_client
*/
void exampleNavigationPointToPoint();
+ void exampleNavigationUpdateNavigator();
+
private:
void goalReached();
struct{
std::string robotName = "Armar6";
float relativeMovement = 200; // [mm]
+
+ Mode mode = Mode::Standard;
} properties;
armarx::RunningTask<Component>::pointer_type task;
diff --git a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
index a64fd58d..7c4574e0 100644
--- a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
+++ b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
@@ -133,14 +133,6 @@ ArmarX.ArticulatedObjectLocalizerDynamicSimulation.mem.obj.articulated.ProviderN
# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.objects = Default value not mapped.
-# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ArticulatedObjectLocalizerDynamicSimulation.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
diff --git a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
index 18e2d2db..aa21b8fd 100644
--- a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
+++ b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
@@ -139,6 +139,23 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = LeftHandUnit
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName = Simulator
+# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory: Require the legacy MemoryX working memory to be available before starting.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory = false
+
+
+# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName: Name of the object pose storage (only used if necessary).
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName = ObjectMemory
+
+
# ArmarX.HandUnitDynamicSimulation.inheritFrom: No Description
# Attributes:
# - Default: RobotConfig
diff --git a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
index 85cc4345..b92bb000 100644
--- a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
+++ b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
@@ -139,6 +139,23 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = RightHandUnit
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName = Simulator
+# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory: Require the legacy MemoryX working memory to be available before starting.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HandUnitDynamicSimulation.UseLegacyWorkingMemory = false
+
+
+# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName: Name of the object pose storage (only used if necessary).
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HandUnitDynamicSimulation.cmp.ObjectPoseStorageName = ObjectMemory
+
+
# ArmarX.HandUnitDynamicSimulation.inheritFrom: No Description
# Attributes:
# - Default: RobotConfig
diff --git a/scenarios/NavigationSimulation/config/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index ed320714..a1b9d81c 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -513,37 +513,21 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
-# ArmarX.ObjectMemory.mem.ltm..configuration:
+# ArmarX.ObjectMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.ltm..configuration = ""
+# ArmarX.ObjectMemory.mem.ltm.configuration = {}
-# ArmarX.ObjectMemory.mem.ltm..enabled:
+# ArmarX.ObjectMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm..enabled = false
-
-
-# ArmarX.ObjectMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.ObjectMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
# ArmarX.ObjectMemory.mem.robot_state.Memory:
diff --git a/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg b/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
index 47323755..10974951 100644
--- a/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
@@ -474,7 +474,7 @@ ArmarX.RobotUnitSimulation.PlatformUnitName = Armar6PlatformUnit
# ArmarX.RobotUnitSimulation.RTLogging_DefaultLog = ""
-# ArmarX.RobotUnitSimulation.RTLogging_KeepIterationsForMs: All logging data (SensorValues, ControlTargets, Messages) is keept for this duration and can be dunped in case of an error.
+# ArmarX.RobotUnitSimulation.RTLogging_KeepIterationsForMs: All logging data (SensorValues, ControlTargets, Messages) is kept for this duration and can be dumped in case of an error.
# Attributes:
# - Default: 60000
# - Case sensitivity: yes
diff --git a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
index 1dab8f00..b8200627 100644
--- a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
@@ -175,23 +175,32 @@ ArmarX.SelfLocalizationDynamicSimulation.RobotName = Armar6
# ArmarX.SelfLocalizationDynamicSimulation.cmp.Simulator = Simulator
-# ArmarX.SelfLocalizationDynamicSimulation.cmp.WorkingMemory: Ice object name of the `WorkingMemory` component.
+# ArmarX.SelfLocalizationDynamicSimulation.cycleTime:
# Attributes:
-# - Default: WorkingMemory
+# - Default: 30
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SelfLocalizationDynamicSimulation.cmp.WorkingMemory = WorkingMemory
+# ArmarX.SelfLocalizationDynamicSimulation.cycleTime = 30
-# ArmarX.SelfLocalizationDynamicSimulation.cycleTime:
+# ArmarX.SelfLocalizationDynamicSimulation.localizationUnit.proxyName: If enabled, connects to the localization unit
# Attributes:
-# - Default: 30
+# - Default: LocalizationUnit
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SelfLocalizationDynamicSimulation.cycleTime = 30
+# ArmarX.SelfLocalizationDynamicSimulation.localizationUnit.proxyName = LocalizationUnit
-# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory: Ice object name of the `LongtermMemory` component.
+# ArmarX.SelfLocalizationDynamicSimulation.localizationUnit.use: If enabled, connects to the localization unit
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SelfLocalizationDynamicSimulation.localizationUnit.use = true
+
+
+# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory: Which legacy long-term memory to use if longterm_memory.updateor longterm_memory.retrieve_initial_pose are set
# Attributes:
# - Default: LongtermMemory
# - Case sensitivity: yes
@@ -291,6 +300,14 @@ ArmarX.SelfLocalizationDynamicSimulation.mem.robot_state.Memory = RobotState
# ArmarX.SelfLocalizationDynamicSimulation.tpc.pub.GlobalRobotPoseLocalization = GlobalRobotPoseLocalization
+# ArmarX.SelfLocalizationDynamicSimulation.tpc.pub.GlobalRobotPoseLocalizationCorrection: Name of the `GlobalRobotPoseLocalizationCorrection` topic to publish data to.
+# Attributes:
+# - Default: GlobalRobotPoseLocalizationCorrection
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SelfLocalizationDynamicSimulation.tpc.pub.GlobalRobotPoseLocalizationCorrection = GlobalRobotPoseLocalizationCorrection
+
+
# ArmarX.SelfLocalizationDynamicSimulation.tpc.pub.LaserScannerSelfLocalisation: Name of the `LaserScannerSelfLocalisation` topic to publish data to.
# Attributes:
# - Default: LaserScannerSelfLocalisation
@@ -307,14 +324,6 @@ ArmarX.SelfLocalizationDynamicSimulation.mem.robot_state.Memory = RobotState
# ArmarX.SelfLocalizationDynamicSimulation.tpc.pub.PlatformUnit = PlatformUnit
-# ArmarX.SelfLocalizationDynamicSimulation.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SelfLocalizationDynamicSimulation.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.SelfLocalizationDynamicSimulation.working_memory.update: If enabled, updates the global pose in the working memory
# Attributes:
# - Default: true
diff --git a/scenarios/NavigationSimulation/config/SimulatorViewerApp.cfg b/scenarios/NavigationSimulation/config/SimulatorViewerApp.cfg
index d514a485..74604be7 100644
--- a/scenarios/NavigationSimulation/config/SimulatorViewerApp.cfg
+++ b/scenarios/NavigationSimulation/config/SimulatorViewerApp.cfg
@@ -117,73 +117,14 @@
# ArmarX.SecondsStartupDelay = 0
-# ArmarX.SimulatorViewer_EntityDrawer.CommonStorageName: Name of the the CommonStorage memory component
+# ArmarX.SimulatorViewer_EntityDrawer.MinimumLoggingLevel: No Description
# Attributes:
-# - Default: CommonStorage
-# - Case sensitivity: yes
+# - Default: Verbose
+# - Case sensitivity: no
# - Required: no
-# ArmarX.SimulatorViewer_EntityDrawer.CommonStorageName = CommonStorage
-
-
-# ArmarX.SimulatorViewer_EntityDrawer.DebugDrawerSelectionTopic: Name of the DebugDrawerSelectionTopic
-# Attributes:
-# - Default: DebugDrawerSelections
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SimulatorViewer_EntityDrawer.DebugDrawerSelectionTopic = DebugDrawerSelections
-
-
-# ArmarX.SimulatorViewer_EntityDrawer.DebugDrawerTopic: Name of the DebugDrawerTopic
-# Attributes:
-# - Default: DebugDrawerUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SimulatorViewer_EntityDrawer.DebugDrawerTopic = DebugDrawerUpdates
-
-
-# ArmarX.SimulatorViewer_EntityDrawer.EnableProfiling: enable profiler which is used for logging performance events
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SimulatorViewer_EntityDrawer.EnableProfiling = false
-
-
-# ArmarX.SimulatorViewer_EntityDrawer.MinimumLoggingLevel: Local logging level only for this component
-# Attributes:
-# - Default: Undefined
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
ArmarX.SimulatorViewer_EntityDrawer.MinimumLoggingLevel = Verbose
-# ArmarX.SimulatorViewer_EntityDrawer.ObjectName: Name of IceGrid well-known object
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SimulatorViewer_EntityDrawer.ObjectName = ""
-
-
-# ArmarX.SimulatorViewer_EntityDrawer.PriorKnowledgeName: Name of the the PriorKnowledge memory component
-# Attributes:
-# - Default: PriorKnowledge
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SimulatorViewer_EntityDrawer.PriorKnowledgeName = PriorKnowledge
-
-
-# ArmarX.SimulatorViewer_EntityDrawer.ShowDebugDrawing: The simulator implements the DebugDrawerInterface. The debug visualizations (e.g. coordinate systems) can enabled/disbaled with this flag.
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SimulatorViewer_EntityDrawer.ShowDebugDrawing = true
-
-
# ArmarX.SimulatorViewer_PhysicsWorldVisualization.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
@@ -383,6 +324,15 @@ ArmarX.SimulatorViewer_SimulationWindow.Camera.z = 6000
# ArmarX.UpdateRate = 30
+# ArmarX.UseDebugDrawer: Whether to create the debug drawer component for the viewer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseDebugDrawer = false
+
+
# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
# - Default: false
diff --git a/scenarios/NavigationSimulation/config/VisionMemory.cfg b/scenarios/NavigationSimulation/config/VisionMemory.cfg
index 4c7327ed..1b95447f 100644
--- a/scenarios/NavigationSimulation/config/VisionMemory.cfg
+++ b/scenarios/NavigationSimulation/config/VisionMemory.cfg
@@ -210,37 +210,21 @@
# ArmarX.VisionMemory.mem.MemoryName = Vision
-# ArmarX.VisionMemory.mem.ltm..configuration:
+# ArmarX.VisionMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.VisionMemory.mem.ltm..configuration = ""
+# ArmarX.VisionMemory.mem.ltm.configuration = {}
-# ArmarX.VisionMemory.mem.ltm..enabled:
+# ArmarX.VisionMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm..enabled = false
-
-
-# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.storagepath = Default value not mapped.
+# ArmarX.VisionMemory.mem.ltm.enabled = false
# ArmarX.VisionMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/ObjectMemory.cfg b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
index 2af60bde..b2764c9b 100644
--- a/scenarios/PlatformNavigation/config/ObjectMemory.cfg
+++ b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
@@ -513,21 +513,74 @@
# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
-# ArmarX.ObjectMemory.mem.ltm..configuration:
+# ArmarX.ObjectMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.ltm..configuration = ""
+# ArmarX.ObjectMemory.mem.ltm..buffer.storeFreq = 10
-# ArmarX.ObjectMemory.mem.ltm..enabled:
+# ArmarX.ObjectMemory.mem.ltm.depthImageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
+# ArmarX.ObjectMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm..enabled = false
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.ltm.exrConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.ObjectMemory.mem.ltm.imageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.imageExtractor.Enabled = true
+
+
+# ArmarX.ObjectMemory.mem.ltm.memFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.memFreqFilter.Enabled = false
+
+
+# ArmarX.ObjectMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.memFreqFilter.WaitingTime = -1
+
+
+# ArmarX.ObjectMemory.mem.ltm.pngConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.pngConverter.Enabled = true
# ArmarX.ObjectMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
@@ -538,6 +591,40 @@
# ArmarX.ObjectMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+# ArmarX.ObjectMemory.mem.ltm.snapEqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.snapEqFilter.Enabled = false
+
+
+# ArmarX.ObjectMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
+
+
+# ArmarX.ObjectMemory.mem.ltm.snapFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.ObjectMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
# ArmarX.ObjectMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
# Attributes:
# - Default: Default value not mapped.
@@ -678,5 +765,3 @@
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.Verbosity = Info
-
-
diff --git a/scenarios/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
index 4c7327ed..0dfef493 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -210,21 +210,74 @@
# ArmarX.VisionMemory.mem.MemoryName = Vision
-# ArmarX.VisionMemory.mem.ltm..configuration:
+# ArmarX.VisionMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.VisionMemory.mem.ltm..configuration = ""
+# ArmarX.VisionMemory.mem.ltm..buffer.storeFreq = 10
-# ArmarX.VisionMemory.mem.ltm..enabled:
+# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
+# ArmarX.VisionMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm..enabled = false
+# ArmarX.VisionMemory.mem.ltm.enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled = true
+
+
+# ArmarX.VisionMemory.mem.ltm.memFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.memFreqFilter.Enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.ltm.memFreqFilter.WaitingTime = -1
+
+
+# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled = true
# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
@@ -235,6 +288,40 @@
# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
+
+
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
# Attributes:
# - Default: Default value not mapped.
@@ -275,5 +362,3 @@
# - Case sensitivity: yes
# - Required: no
# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
-
-
diff --git a/scenarios/PlatformNavigation/config/example_client.cfg b/scenarios/PlatformNavigation/config/example_client.cfg
index eef85301..6a9bccd3 100644
--- a/scenarios/PlatformNavigation/config/example_client.cfg
+++ b/scenarios/PlatformNavigation/config/example_client.cfg
@@ -102,40 +102,6 @@
# ArmarX.ExampleClient.ObjectName = ""
-# ArmarX.ExampleClient.mem.robot_state.Memory:
-# Attributes:
-# - Default: RobotState
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleClient.mem.robot_state.Memory = RobotState
-
-
-# ArmarX.ExampleClient.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
-# Attributes:
-# - Default: Localization
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleClient.mem.robot_state.localizationSegment = Localization
-
-
-# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled = true
-
-
-# ArmarX.ExampleClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
-# Attributes:
-# - Default: MemoryNameSystem
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleClient.mns.MemoryNameSystemName = MemoryNameSystem
-
-
# ArmarX.ExampleClient.nav.NavigatorName: Name of the Navigator
# Attributes:
# - Default: navigator
@@ -242,5 +208,3 @@ ArmarX.ExampleClient.nav.NavigatorName = navigator
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.Verbosity = Info
-
-
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index f808f777..8253a316 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -155,7 +155,7 @@
# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.configuration = {}
+# ArmarX.NavigationMemory.mem.ltm..configuration = ""
# ArmarX.NavigationMemory.mem.ltm.enabled:
@@ -164,7 +164,23 @@
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.mem.ltm.enabled = false
+# ArmarX.NavigationMemory.mem.ltm..enabled = false
+
+
+# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+
+
+# ArmarX.NavigationMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NavigationMemory.mem.ltm.storagepath = Default value not mapped.
# ArmarX.NavigationMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
@@ -286,5 +302,3 @@ ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graph
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.Verbosity = Info
-
-
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 482582bb..233f17f4 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -139,12 +139,20 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+ArmarX.Navigator.ObjectName = navigator
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.Navigator.RobotUnitName: Name of the RobotUnit
# Attributes:
-# - Default: false
+# - Default: Armar6Unit
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.RobotUnitName = Armar6Unit
+
+
+# ArmarX.Navigator.cmp.PlatformUnit: Ice object name of the `PlatformUnit` component.
+# Attributes:
+# - Default: PlatformUnit
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
@@ -270,18 +278,26 @@ ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigator.mem.nav.events.Provider = ""
+# ArmarX.Navigator.mem.nav.stack_result.Provider = ""
-# ArmarX.navigator.mem.nav.graph.CoreSegment:
+# ArmarX.Navigator.mem.robot_state.Memory:
# Attributes:
-# - Default: Location
+# - Default: RobotState
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigator.mem.nav.graph.CoreSegment = Location
+# ArmarX.Navigator.mem.robot_state.Memory = RobotState
-# ArmarX.navigator.mem.nav.graph.Memory:
+# ArmarX.Navigator.mem.robot_state.descriptionSegment:
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Navigator.mem.robot_state.descriptionSegment = Description
+
+
+# ArmarX.Navigator.mem.robot_state.localizationSegment:
# Attributes:
# - Default: Navigation
# - Case sensitivity: yes
@@ -289,15 +305,16 @@ ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.navigator.mem.nav.graph.Memory = Navigation
-# ArmarX.navigator.mem.nav.param.CoreSegment:
+# ArmarX.Navigator.mem.robot_state.proprioceptionSegment:
# Attributes:
-# - Default: Parameterization
+# - Default: Proprioception
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigator.mem.nav.param.CoreSegment = Parameterization
+# ArmarX.Navigator.mem.robot_state.proprioceptionSegment = Proprioception
-# ArmarX.navigator.mem.nav.param.Memory:
+# ArmarX.Navigator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
# Attributes:
# - Default: Navigation
# - Case sensitivity: yes
@@ -321,15 +338,15 @@ ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.navigator.mem.nav.stack_result.CoreSegment = ""
-# ArmarX.navigator.mem.nav.stack_result.Memory:
+# ArmarX.Navigator.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
# Attributes:
-# - Default: Navigation
+# - Default: MemoryUpdates
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigator.mem.nav.stack_result.Memory = Navigation
+# ArmarX.Navigator.tpc.sub.MemoryListener = MemoryUpdates
-# ArmarX.navigator.mem.nav.stack_result.Provider: Name of this provider
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: ""
# - Case sensitivity: yes
@@ -386,5 +403,3 @@ ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
# - Case sensitivity: yes
# - Required: no
ArmarX.navigator.p.occupancy_grid.occopied_threshold = 0.8
-
-
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index b047700c..173e3dd4 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -17,93 +17,97 @@
// ComponentPlugin
-armarx::navigation::client::ComponentPlugin::~ComponentPlugin() = default;
-
-void
-armarx::navigation::client::ComponentPlugin::postCreatePropertyDefinitions(
- armarx::PropertyDefinitionsPtr& properties)
+namespace armarx::navigation::client
{
- if (not properties->hasDefinition(PROPERTY_NAME))
- {
- properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME, "navigator", "Name of the Navigator");
- }
-}
-void
-armarx::navigation::client::ComponentPlugin::preOnInitComponent()
-{
- ARMARX_TRACE;
- parent<armarx::Component>().usingProxyFromProperty(PROPERTY_NAME);
-}
+ ComponentPlugin::~ComponentPlugin() = default;
-void
-armarx::navigation::client::ComponentPlugin::preOnConnectComponent()
-{
- ARMARX_TRACE;
- parent<armarx::Component>().getProxyFromProperty(navigatorPrx, PROPERTY_NAME);
+ void
+ ComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (not properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME, "navigator", "Name of the Navigator");
+ }
+ }
- ARMARX_TRACE;
- const std::string componentName = parent().getName();
+ void
+ ComponentPlugin::preOnInitComponent()
+ {
+ ARMARX_TRACE;
+ parent<armarx::Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
- ARMARX_CHECK_NOT_NULL(navigatorPrx) << "Navigator proxy is null!";
- iceNavigator.setNavigatorComponent(navigatorPrx);
-}
+ void
+ ComponentPlugin::preOnConnectComponent()
+ {
+ ARMARX_TRACE;
+ parent<armarx::Component>().getProxyFromProperty(navigatorPrx, PROPERTY_NAME);
-void
-armarx::navigation::client::ComponentPlugin::configureNavigator(
- const client::NavigationStackConfig& stackConfig,
- const std::string& configId)
-{
- ARMARX_TRACE;
+ ARMARX_TRACE;
+ const std::string componentName = parent().getName();
- // ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
+ ARMARX_CHECK_NOT_NULL(navigatorPrx) << "Navigator proxy is null!";
+ iceNavigator.setNavigatorComponent(navigatorPrx);
+ }
- eventHandler = [&]() -> std::unique_ptr<SimpleEventHandler>
+ void
+ ComponentPlugin::configureNavigator(const client::NavigationStackConfig& stackConfig,
+ const std::string& configId)
{
- if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
- {
- ARMARX_INFO << "Using memory event callbacks.";
- // must(!!) use a reference here: otherwise the mns events won't work
- auto& memoryNameSystem =
- parent<armarx::armem::client::plugins::ListeningPluginUser>().memoryNameSystem();
- return std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
- }
+ ARMARX_TRACE;
- ARMARX_INFO << "Cannot use memory event callbacks as this component is not derived from "
- "`armarx::armem::client::plugins::ListeningPluginUser`";
+ // ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
- return std::make_unique<SimpleEventHandler>();
- }();
-
- iceNavigator.createConfig(stackConfig, configId);
+ eventHandler = [&]() -> std::unique_ptr<SimpleEventHandler>
+ {
+ if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
+ {
+ ARMARX_INFO << "Using memory event callbacks.";
+ // must(!!) use a reference here: otherwise the mns events won't work
+ 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()});
+ }
- navigator = std::make_unique<Navigator>(
- Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
-}
+ // ComponentPluginUser
-// ComponentPluginUser
+ ComponentPluginUser::ComponentPluginUser()
+ {
+ ARMARX_TRACE;
+ addPlugin(plugin);
+ }
-armarx::navigation::client::ComponentPluginUser::ComponentPluginUser()
-{
- ARMARX_TRACE;
- addPlugin(plugin);
-}
+ void
+ ComponentPluginUser::configureNavigator(const client::NavigationStackConfig& stackConfig)
+ {
+ ARMARX_TRACE;
+ plugin->configureNavigator(stackConfig, getName());
+ }
-void
-armarx::navigation::client::ComponentPluginUser::configureNavigator(
- const client::NavigationStackConfig& stackConfig)
-{
- ARMARX_TRACE;
- plugin->configureNavigator(stackConfig, getName());
-}
+ Navigator&
+ 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::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;
-}
+ ComponentPluginUser::~ComponentPluginUser() = default;
-armarx::navigation::client::ComponentPluginUser::~ComponentPluginUser() = default;
+} // namespace armarx::navigation::client
diff --git a/source/armarx/navigation/client/Navigator.cpp b/source/armarx/navigation/client/Navigator.cpp
index 15fb4bc1..e2f47c4d 100644
--- a/source/armarx/navigation/client/Navigator.cpp
+++ b/source/armarx/navigation/client/Navigator.cpp
@@ -60,6 +60,13 @@ namespace armarx::navigation::client
srv.navigator->moveTo(path, frame);
}
+
+ void Navigator::update(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->update(waypoints, frame);
+ }
void
diff --git a/source/armarx/navigation/client/Navigator.h b/source/armarx/navigation/client/Navigator.h
index d116a1de..32af0bb9 100644
--- a/source/armarx/navigation/client/Navigator.h
+++ b/source/armarx/navigation/client/Navigator.h
@@ -132,6 +132,8 @@ namespace armarx::navigation::client
void moveTowards(const core::Direction& direction, core::NavigationFrame frame);
+ void update(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame);
+
void pause();
void resume();
diff --git a/source/armarx/navigation/client/ice/NavigatorInterface.ice b/source/armarx/navigation/client/ice/NavigatorInterface.ice
index 678379d0..806aa2d9 100644
--- a/source/armarx/navigation/client/ice/NavigatorInterface.ice
+++ b/source/armarx/navigation/client/ice/NavigatorInterface.ice
@@ -67,6 +67,8 @@ module armarx
void moveTo2(detail::Waypoints waypoints, string navigationFrame, string callerId);
+ void updateMoveTo(Eigen::Matrix4fSeq waypoints, string navigationFrame, string callerId);
+
void
moveTowards(Eigen::Vector3f direction, string navigationFrame, string callerId);
diff --git a/source/armarx/navigation/client/services/IceNavigator.cpp b/source/armarx/navigation/client/services/IceNavigator.cpp
index 965a59ee..cc1c51b4 100644
--- a/source/armarx/navigation/client/services/IceNavigator.cpp
+++ b/source/armarx/navigation/client/services/IceNavigator.cpp
@@ -2,6 +2,8 @@
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include "armarx/navigation/core/types.h"
+
namespace armarx::navigation::client
{
@@ -79,6 +81,16 @@ namespace armarx::navigation::client
direction, core::NavigationFrameNames.to_name(navigationFrame), configId);
}
+ void
+ IceNavigator::update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame)
+ {
+ ARMARX_TRACE;
+
+ navigator->updateMoveTo(
+ convert(waypoints), core::NavigationFrameNames.to_name(navigationFrame), configId);
+ }
+
void
IceNavigator::pause()
{
diff --git a/source/armarx/navigation/client/services/IceNavigator.h b/source/armarx/navigation/client/services/IceNavigator.h
index 60750002..142bebb4 100644
--- a/source/armarx/navigation/client/services/IceNavigator.h
+++ b/source/armarx/navigation/client/services/IceNavigator.h
@@ -3,6 +3,8 @@
#include <string>
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/core/types.h"
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/ice_conversions.h>
@@ -35,6 +37,9 @@ namespace armarx::navigation::client
void moveTowards(const core::Direction& direction,
core::NavigationFrame navigationFrame) override;
+ void update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame) override;
+
void pause() override;
void resume() override;
diff --git a/source/armarx/navigation/components/navigator/CMakeLists.txt b/source/armarx/navigation/components/navigator/CMakeLists.txt
index a8d6245e..a686acda 100644
--- a/source/armarx/navigation/components/navigator/CMakeLists.txt
+++ b/source/armarx/navigation/components/navigator/CMakeLists.txt
@@ -8,7 +8,8 @@ armarx_add_component(navigator
DEPENDENCIES
# ArmarXCore
ArmarXCore
- # ArmarXCoreComponentPlugins # For DebugObserver plugin. ArmarXGui
+ ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
RobotAPICore
@@ -18,15 +19,13 @@ armarx_add_component(navigator
armem_robot
armem_robot_state
armem_vision
- # This project ${PROJECT_NAME}Interfaces # For ice interfaces from this
- # package. This component
+ # navigation
armarx_navigation::server
armarx_navigation::client
armarx_navigation::util
armarx_navigation::factories
+ # Simox
Simox::SimoxUtility
+ # others
Eigen3::Eigen
- ICE_DEPENDENCIES
- ArmarXCoreInterfaces
- RobotAPIInterfaces
)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 4cf28549..32cc481f 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -281,6 +281,7 @@ namespace armarx::navigation::components::navigator
.publisher =
memoryPublishers.at(callerId).get(),
.introspector = &(introspector.value()),
+ .debugObserverHelper = &getDebugObserverComponentPlugin(),
.sceneProvider = &sceneProvider.value()}));
}
@@ -313,6 +314,37 @@ namespace armarx::navigation::components::navigator
}
}
+ void
+ Navigator::updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current& /*c*/)
+ {
+ ARMARX_TRACE;
+
+ ARMARX_INFO << "updateMoveTo() requested by caller '" << callerId << "'";
+ ARMARX_CHECK(navigators.count(callerId) > 0)
+ << "Navigator config for caller `" << callerId << "` not registered!";
+
+ const auto checkIsActiveNavigator = [&](const std::string& callerId)
+ {
+ const std::optional<std::string> activeNavigatorCallerId = activeNavigator();
+ if (not activeNavigatorCallerId.has_value())
+ {
+ ARMARX_VERBOSE << "No navigator is active.";
+ return false;
+ }
+
+ return activeNavigatorCallerId == callerId;
+ };
+
+ ARMARX_CHECK(checkIsActiveNavigator(callerId))
+ << "The navigator with id `" << callerId << "` is not active!";
+
+ navigators.at(callerId).update(convert(waypoints),
+ core::NavigationFrameNames.from_name(navigationMode));
+ }
+
void
Component::moveTo2(const client::detail::Waypoints& waypoints,
@@ -458,27 +490,67 @@ namespace armarx::navigation::components::navigator
return def;
}
+ void
+ visualize(const algorithms::Costmap& costmap, viz::Client& arviz, const std::string& name)
+ {
+ const auto cmap = simox::color::cmaps::viridis();
+ const float vmax = costmap.getGrid().array().maxCoeff();
+
+ const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
+ {
+ const auto color = cmap.at(distance, 0.F, vmax);
+ return {color.a, color.r, color.g, color.b};
+ };
+
+ auto layer = arviz.layer("costmap_" + name);
+
+ const std::int64_t cols = costmap.getGrid().cols();
+ const std::int64_t rows = costmap.getGrid().rows();
+
+ auto mesh = viz::Mesh("");
+
+ std::vector<std::vector<Eigen::Vector3f>> vertices;
+ std::vector<std::vector<viz::data::Color>> colors;
+
+ for (int r = 0; r < rows; r++)
+ {
+ auto& verticesRow = vertices.emplace_back(cols);
+ auto& colorsRow = colors.emplace_back(cols);
+
+ for (int c = 0; c < cols; c++)
+ {
+ verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
+ colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
+ }
+ }
+
+ mesh.grid2D(vertices, colors);
+
+ layer.add(mesh);
+
+ arviz.commit(layer);
+ }
server::Navigator*
- Component::activeNavigator()
+ components::Navigator::activeNavigator()
{
ARMARX_TRACE;
// We define the active navigator to be the one whose movement is enabled.
- const auto isActive = [](auto& nameNavPair) -> bool
+ const auto isActive = [](const auto& nameNavPair) -> bool
{
- server::Navigator& navigator = nameNavPair.second;
+ const auto& [name, navigator] = nameNavPair;
return not navigator.isPaused();
};
- auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
+ const auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
// no navigator active?
if (it == navigators.end())
{
- return nullptr;
+ return std::nullopt;
}
- return &it->second;
+ return it->first;
}
} // namespace armarx::navigation::components::navigator
diff --git a/source/armarx/navigation/components/navigator/Component.h b/source/armarx/navigation/components/navigator/Component.h
index 316e4da5..d5f41ea9 100644
--- a/source/armarx/navigation/components/navigator/Component.h
+++ b/source/armarx/navigation/components/navigator/Component.h
@@ -33,6 +33,7 @@
#include <Ice/Object.h>
+#include "ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
@@ -85,7 +86,8 @@ namespace armarx::navigation::components::navigator
virtual public client::NavigatorInterface,
virtual public ObjectPoseClientPluginUser,
virtual public ArVizComponentPluginUser,
- virtual public armarx::control::client::ComponentPluginUser
+ virtual public armarx::control::client::ComponentPluginUser,
+ virtual public armarx::DebugObserverComponentPluginUser
// virtual public armem::ListeningClientPluginUser
{
@@ -112,6 +114,11 @@ namespace armarx::navigation::components::navigator
const std::string& callerId,
const Ice::Current& c = Ice::emptyCurrent) override;
+ void updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+
void moveTowards(const Eigen::Vector3f& direction,
const std::string& navigationMode,
const std::string& callerId,
@@ -158,7 +165,7 @@ namespace armarx::navigation::components::navigator
// [[nodiscard]] VirtualRobot::RobotPtr getRobot();
// void updateRobot();
- server::Navigator* activeNavigator();
+ std::optional<std::string> activeNavigator() const;
private:
void visualizeSPFA();
diff --git a/source/armarx/navigation/core/NavigatorInterface.h b/source/armarx/navigation/core/NavigatorInterface.h
index 05df688d..18e50335 100644
--- a/source/armarx/navigation/core/NavigatorInterface.h
+++ b/source/armarx/navigation/core/NavigatorInterface.h
@@ -6,6 +6,10 @@
namespace armarx::navigation::core
{
+ /**
+ * @brief Navigator interface for PointGoal navigation (with waypoints) and relative movement
+ *
+ */
class NavigatorInterface
{
@@ -19,6 +23,8 @@ namespace armarx::navigation::core
virtual void moveTo(const std::vector<client::WaypointTarget>& targets,
core::NavigationFrame navigationFrame) = 0;
+ virtual void update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame) = 0;
virtual void pause() = 0;
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index af121508..22913fa5 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -5,9 +5,6 @@
#include <optional>
#include <thread>
-#include <range/v3/view.hpp>
-#include <range/v3/view/reverse.hpp>
-
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
@@ -42,6 +39,8 @@
#include <armarx/navigation/server/StackResult.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
+#include <range/v3/view.hpp>
+#include <range/v3/view/reverse.hpp>
namespace armarx::navigation::server
@@ -164,6 +163,33 @@ namespace armarx::navigation::server
moveToAbsolute(globalWaypoints);
}
+ void
+ Navigator::update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame)
+ {
+ ARMARX_INFO << "Received update() request.";
+
+ std::vector<core::Pose> globalWaypoints;
+ switch (navigationFrame)
+ {
+ case core::NavigationFrame::Absolute:
+ globalWaypoints = waypoints;
+ break;
+ case core::NavigationFrame::Relative:
+ globalWaypoints.reserve(waypoints.size());
+ std::transform(
+ std::begin(waypoints),
+ std::end(waypoints),
+ std::back_inserter(globalWaypoints),
+ [&](const core::Pose& p)
+ { return core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()) * p; });
+ break;
+ }
+
+ ARMARX_TRACE;
+ updateAbsolute(globalWaypoints);
+ }
+
using GraphPath = std::vector<semrel::ShapeID>;
@@ -626,6 +652,8 @@ namespace armarx::navigation::server
ARMARX_TRACE;
ARMARX_INFO << "Planning local trajectory enabled";
+
+ // FIXME instead of PeriodicTask, use RunningTask.
runningTask = new PeriodicTask<Navigator>(this,
&Navigator::run,
config.general.tasks.replanningUpdatePeriod,
@@ -644,13 +672,14 @@ namespace armarx::navigation::server
Navigator::moveToAbsolute(const std::vector<core::Pose>& waypoints)
{
ARMARX_TRACE;
+
// if this navigator is in use, stop the movement ...
pause();
// ... and all threads
stopAllThreads();
// FIXME remove
- std::this_thread::sleep_for(std::chrono::seconds(1));
+ //std::this_thread::sleep_for(std::chrono::seconds(1));
ARMARX_TRACE;
ARMARX_CHECK_NOT_EMPTY(waypoints);
@@ -710,6 +739,48 @@ namespace armarx::navigation::server
ARMARX_INFO << "Movement started.";
}
+
+ void
+ Navigator::updateAbsolute(const std::vector<core::Pose>& waypoints)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_EMPTY(waypoints);
+
+
+ // global planner
+ ARMARX_INFO << "Planning global trajectory";
+ ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
+ // TODO plan on multiple waypoints, ignoring waypoints for now
+ // idea: compute multiple global trajectories, one for each segment between waypoints.
+
+ srv.introspector->onGoal(waypoints.back());
+ globalPlan = config.stack.globalPlanner->plan(waypoints.back());
+
+ ARMARX_TRACE;
+
+ if (not globalPlan.has_value())
+ {
+ ARMARX_WARNING << "No global trajectory. Cannot move.";
+ srv.publisher->globalPlanningFailed(
+ core::GlobalPlanningFailedEvent{{.timestamp = armarx::Clock::Now()}, {""}});
+
+ srv.introspector->failure();
+ return;
+ }
+
+ ARMARX_TRACE;
+ srv.publisher->globalTrajectoryUpdated(globalPlan.value());
+ srv.introspector->onGlobalPlannerResult(globalPlan.value());
+ // srv.executor->execute(globalPlan->trajectory);
+
+ ARMARX_INFO << "Global planning completed. Will now start all required threads";
+ ARMARX_TRACE;
+
+ startStack();
+
+ ARMARX_INFO << "Movement started.";
+ }
+
void
Navigator::moveTowards(const core::Direction& direction, core::NavigationFrame navigationFrame)
{
@@ -733,8 +804,67 @@ namespace armarx::navigation::server
const Duration duration =
armarx::core::time::StopWatch::measure([&]() { updateScene(); });
- ARMARX_VERBOSE << deactivateSpam(0.2) << "Scene update: " << duration.toMilliSecondsDouble()
- << "ms.";
+ ARMARX_VERBOSE << deactivateSpam(0.2)
+ << "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
+
+ srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]", duration.toMilliSecondsDouble());
+ }
+
+ // global planner update if goal has changed
+ {
+ std::lock_guard g{globalPlanningRequestMtx};
+
+ if (globalPlanningRequest.has_value())
+ {
+ const auto& waypoints = globalPlanningRequest.value();
+
+ // recreate goal monitor
+ {
+ // first we check if we are already at the goal position
+ goalReachedMonitor = std::nullopt;
+ goalReachedMonitor = GoalReachedMonitor(
+ waypoints.back(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
+
+ if (goalReachedMonitor->goalReached())
+ {
+ ARMARX_INFO << "Already at goal position. Robot won't move.";
+
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
+
+ return;
+ }
+ }
+
+ // global planning
+ {
+ // global planner
+ ARMARX_INFO << "Update/Planning global trajectory";
+ ARMARX_CHECK_NOT_NULL(config.stack.globalPlanner);
+ // TODO plan on multiple waypoints, ignoring waypoints for now
+ // idea: compute multiple global trajectories, one for each segment between waypoints.
+
+ srv.introspector->onGoal(waypoints.back());
+ globalPlan = config.stack.globalPlanner->plan(waypoints.back());
+
+ ARMARX_TRACE;
+
+ if (not globalPlan.has_value())
+ {
+ ARMARX_WARNING << "No global trajectory. Cannot move.";
+ srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
+ {.timestamp = armarx::Clock::Now()}, {""}});
+
+ srv.introspector->failure();
+ return;
+ }
+
+ ARMARX_TRACE;
+ srv.publisher->globalTrajectoryUpdated(globalPlan.value());
+ srv.introspector->onGlobalPlannerResult(globalPlan.value());
+ }
+ }
}
// local planner update
@@ -753,6 +883,8 @@ namespace armarx::navigation::server
});
ARMARX_VERBOSE << deactivateSpam(0.2)
<< "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
+
+ srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]", duration.toMilliSecondsDouble());
}
// monitor update
@@ -764,6 +896,8 @@ namespace armarx::navigation::server
ARMARX_VERBOSE << deactivateSpam(0.2)
<< "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
+
+ srv.debugObserverHelper->setDebugObserverDatafield("monitor update [ms]", duration.toMilliSecondsDouble());
}
}
@@ -796,7 +930,7 @@ namespace armarx::navigation::server
void
Navigator::updateExecutor(const std::optional<local_planning::LocalPlannerResult>& localPlan)
{
- if(srv.executor == nullptr)
+ if (srv.executor == nullptr)
{
return;
}
@@ -835,7 +969,8 @@ namespace armarx::navigation::server
}
void
- Navigator::updateIntrospector(const std::optional<local_planning::LocalPlannerResult>& localPlan)
+ Navigator::updateIntrospector(
+ const std::optional<local_planning::LocalPlannerResult>& localPlan)
{
ARMARX_CHECK_NOT_NULL(srv.introspector);
@@ -930,7 +1065,7 @@ namespace armarx::navigation::server
executorEnabled.store(false);
- if(srv.executor != nullptr)
+ if (srv.executor != nullptr)
{
srv.executor->stop();
}
@@ -943,7 +1078,7 @@ namespace armarx::navigation::server
executorEnabled.store(true);
- if(srv.executor != nullptr)
+ if (srv.executor != nullptr)
{
srv.executor->start();
}
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index 5c236f3a..f4c0fb10 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -30,6 +30,7 @@
#include <Eigen/Core>
// ArmarX
+#include "ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
@@ -81,6 +82,8 @@ namespace armarx::navigation::server
EventPublishingInterface* publisher;
IntrospectorInterface* introspector = nullptr;
+ armarx::DebugObserverHelper* debugObserverHelper;
+
scene_provider::SceneProviderInterface* sceneProvider;
};
@@ -89,6 +92,9 @@ namespace armarx::navigation::server
void moveTo(const std::vector<core::Pose>& waypoints,
core::NavigationFrame navigationFrame) override;
+ void update(const std::vector<core::Pose>& waypoints,
+ core::NavigationFrame navigationFrame) override;
+
void moveTo(const std::vector<client::WaypointTarget>& targets,
core::NavigationFrame navigationFrame) override;
@@ -116,6 +122,8 @@ namespace armarx::navigation::server
void moveToAbsolute(const std::vector<core::Pose>& waypoints);
void moveTowardsAbsolute(const core::Direction& direction);
+ void updateAbsolute(const std::vector<core::Pose>& waypoints);
+
void run();
void updateScene();
@@ -155,6 +163,11 @@ namespace armarx::navigation::server
std::optional<global_planning::GlobalPlannerResult> globalPlan;
std::optional<local_planning::LocalPlannerResult> localPlan;
+
+ using Waypoints = std::vector<core::Pose>;
+
+ std::mutex globalPlanningRequestMtx;
+ std::optional<Waypoints> globalPlanningRequest;
};
} // namespace armarx::navigation::server
--
GitLab
From 0bf689dda741a622cedcf1d8f28cd5660327993a Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 19 Aug 2022 19:40:59 +0200
Subject: [PATCH 2/7] minor cleanup
---
.../components/navigator/Component.cpp | 16 +++++++++++++++-
.../navigation/components/navigator/Component.h | 3 ++-
2 files changed, 17 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 32cc481f..d4938415 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -29,6 +29,7 @@
#include <cstdint>
#include <iterator>
#include <memory>
+#include <optional>
#include <utility>
#include <Eigen/Geometry>
@@ -532,7 +533,20 @@ namespace armarx::navigation::components::navigator
}
server::Navigator*
- components::Navigator::activeNavigator()
+ Component::activeNavigator()
+ {
+
+ const auto navigatorId = activeNavigatorId();
+ if(not navigatorId.has_value())
+ {
+ return nullptr;
+ }
+
+ return &navigators.at(navigatorId.value());
+ }
+
+ std::optional<std::string>
+ Component::activeNavigatorId() const
{
ARMARX_TRACE;
// We define the active navigator to be the one whose movement is enabled.
diff --git a/source/armarx/navigation/components/navigator/Component.h b/source/armarx/navigation/components/navigator/Component.h
index d5f41ea9..5e552d7f 100644
--- a/source/armarx/navigation/components/navigator/Component.h
+++ b/source/armarx/navigation/components/navigator/Component.h
@@ -165,7 +165,8 @@ namespace armarx::navigation::components::navigator
// [[nodiscard]] VirtualRobot::RobotPtr getRobot();
// void updateRobot();
- std::optional<std::string> activeNavigator() const;
+ server::Navigator* activeNavigator();
+ std::optional<std::string> activeNavigatorId() const;
private:
void visualizeSPFA();
--
GitLab
From 5801fb3c6e0bb280d307631cdfb413c257d4cf90 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 19 Aug 2022 19:43:01 +0200
Subject: [PATCH 3/7] fix: build
---
source/armarx/navigation/components/navigator/Component.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index d4938415..402c90e4 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -316,7 +316,7 @@ namespace armarx::navigation::components::navigator
}
void
- Navigator::updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+ Component::updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
const std::string& navigationMode,
const std::string& callerId,
const Ice::Current& /*c*/)
@@ -329,7 +329,7 @@ namespace armarx::navigation::components::navigator
const auto checkIsActiveNavigator = [&](const std::string& callerId)
{
- const std::optional<std::string> activeNavigatorCallerId = activeNavigator();
+ const std::optional<std::string> activeNavigatorCallerId = activeNavigatorId();
if (not activeNavigatorCallerId.has_value())
{
ARMARX_VERBOSE << "No navigator is active.";
--
GitLab
From 244b5edbead131343d3de4b4b6dea967f2cf5374 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 19 Aug 2022 19:47:53 +0200
Subject: [PATCH 4/7] scenario update: removing memoryx
---
.../NavigationSimulation.scx | 11 --------
.../config/ObjectMemory.cfg | 25 +++++++++++++------
.../SelfLocalizationDynamicSimulationApp.cfg | 4 +--
3 files changed, 19 insertions(+), 21 deletions(-)
diff --git a/scenarios/NavigationSimulation/NavigationSimulation.scx b/scenarios/NavigationSimulation/NavigationSimulation.scx
index 66b83347..5115bad6 100644
--- a/scenarios/NavigationSimulation/NavigationSimulation.scx
+++ b/scenarios/NavigationSimulation/NavigationSimulation.scx
@@ -3,12 +3,7 @@
<application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ConditionHandler" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SystemObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="CommonStorage" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="PriorKnowledge" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="LongtermMemory" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="WorkingMemory" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ViewSelectionApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ObjectLocalizationSaliencyApp" instance="" package="RobotComponents" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="SimulatorApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SimulatorViewerApp" instance="" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
@@ -19,25 +14,19 @@
<application name="DepthImageProviderDynamicSimulationApp" instance="" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ObjectLocalizationDynamicSimulationApp" instance="TexturedRecognition" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ObjectLocalizationDynamicSimulationApp" instance="SegmentableRecognition" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="ObjectMemoryObserver" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="HandUnitDynamicSimulationApp" instance="LeftHand" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="HandUnitDynamicSimulationApp" instance="RightHand" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ForceTorqueObserver" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SelfLocalizationDynamicSimulationApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotHandLocalizationDynamicSimulationApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="HeadIKUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="TCPControlUnit" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ProfilerObserverApp" instance="" package="ArmarXCore" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="GraphNodePoseResolverApp" instance="" package="MemoryX" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ImageSourceSelectionApp" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="RobotUnitSimulationApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="LaserScannerUnitObserverApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ImageProviderDynamicSimulationApp" instance="Roboception" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArmarXFileLoggerApp" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="WorkingMemoryToArVizApp" instance="" package="MemoryX" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="ObstacleAvoidingPlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="DebugDrawerToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArticulatedObjectLocalizerDynamicSimulation" instance="" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
diff --git a/scenarios/NavigationSimulation/config/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index a1b9d81c..9d742ccc 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -259,6 +259,15 @@ ArmarX.ObjectMemory.MinimumLoggingLevel = Debug
# ArmarX.ObjectMemory.mem.inst.calibration.offset = 0
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName: Name of robot whose note can be calibrated.
+# If not given, the 'fallbackName' is used.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName = ""
+
+
# ArmarX.ObjectMemory.mem.inst.calibration.robotNode: Robot node which can be calibrated.
# Attributes:
# - Default: Neck_2_Pitch
@@ -341,6 +350,14 @@ ArmarX.ObjectMemory.MinimumLoggingLevel = Debug
# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName: Robot name to use as fallback if the robot name is not specified in a provided object pose.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName = ""
+
+
# ArmarX.ObjectMemory.mem.inst.scene.10_Package: ArmarX package containing the scene snapshots.
# Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.
# Attributes:
@@ -572,14 +589,6 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.prediction.TimeWindow = 2
-# ArmarX.ObjectMemory.robotName:
-# Attributes:
-# - Default: Armar6
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.robotName = Armar6
-
-
# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
# - Default: DebugObserver
diff --git a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
index b8200627..aa388636 100644
--- a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
@@ -223,7 +223,7 @@ ArmarX.SelfLocalizationDynamicSimulation.longterm_memory.retrieve_initial_pose =
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory.update = true
+ArmarX.SelfLocalizationDynamicSimulation.longterm_memory.update = false
# ArmarX.SelfLocalizationDynamicSimulation.longterm_memory.update_frequency:
@@ -330,7 +330,7 @@ ArmarX.SelfLocalizationDynamicSimulation.mem.robot_state.Memory = RobotState
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SelfLocalizationDynamicSimulation.working_memory.update = true
+ArmarX.SelfLocalizationDynamicSimulation.working_memory.update = false
# ArmarX.SelfLocalizationDynamicSimulation.working_memory.update_frequency:
--
GitLab
From 236e4b3b9e82dcbe6689473fcf67a99f4c585abe Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 19 Aug 2022 20:25:25 +0200
Subject: [PATCH 5/7] scenario updates: fixes after renaming components
---
.../config/ObjectMemory.cfg | 51 +++---
.../config/VisionMemory.cfg | 26 +--
..._distance_to_obstacle_costmap_provider.cfg | 2 +-
.../config/example_client.cfg | 149 +++++++++-------
.../config/navigation_memory.cfg | 5 +-
.../HumanAwareNavigation/config/navigator.cfg | 68 +++----
.../PlatformNavigation/PlatformNavigation.scx | 2 +-
.../config/ObjectMemory.cfg | 111 +-----------
.../config/VisionMemory.cfg | 111 +-----------
.../config/example_client.cfg | 150 ++++++++++------
.../config/navigation_memory.cfg | 167 +++++++++---------
.../PlatformNavigation/config/navigator.cfg | 77 +++++---
12 files changed, 376 insertions(+), 543 deletions(-)
diff --git a/scenarios/HumanAwareNavigation/config/ObjectMemory.cfg b/scenarios/HumanAwareNavigation/config/ObjectMemory.cfg
index 2af60bde..773e4812 100644
--- a/scenarios/HumanAwareNavigation/config/ObjectMemory.cfg
+++ b/scenarios/HumanAwareNavigation/config/ObjectMemory.cfg
@@ -259,6 +259,15 @@
# ArmarX.ObjectMemory.mem.inst.calibration.offset = 0
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName: Name of robot whose note can be calibrated.
+# If not given, the 'fallbackName' is used.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName = ""
+
+
# ArmarX.ObjectMemory.mem.inst.calibration.robotNode: Robot node which can be calibrated.
# Attributes:
# - Default: Neck_2_Pitch
@@ -341,6 +350,14 @@
# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName: Robot name to use as fallback if the robot name is not specified in a provided object pose.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName = ""
+
+
# ArmarX.ObjectMemory.mem.inst.scene.10_Package: ArmarX package containing the scene snapshots.
# Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.
# Attributes:
@@ -513,37 +530,21 @@
# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
-# ArmarX.ObjectMemory.mem.ltm..configuration:
+# ArmarX.ObjectMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.ltm..configuration = ""
+# ArmarX.ObjectMemory.mem.ltm.configuration = {}
-# ArmarX.ObjectMemory.mem.ltm..enabled:
+# ArmarX.ObjectMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm..enabled = false
-
-
-# ArmarX.ObjectMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.ObjectMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
# ArmarX.ObjectMemory.mem.robot_state.Memory:
@@ -588,14 +589,6 @@
# ArmarX.ObjectMemory.prediction.TimeWindow = 2
-# ArmarX.ObjectMemory.robotName:
-# Attributes:
-# - Default: Armar6
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.robotName = Armar6
-
-
# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
# - Default: DebugObserver
diff --git a/scenarios/HumanAwareNavigation/config/VisionMemory.cfg b/scenarios/HumanAwareNavigation/config/VisionMemory.cfg
index 4c7327ed..1b95447f 100644
--- a/scenarios/HumanAwareNavigation/config/VisionMemory.cfg
+++ b/scenarios/HumanAwareNavigation/config/VisionMemory.cfg
@@ -210,37 +210,21 @@
# ArmarX.VisionMemory.mem.MemoryName = Vision
-# ArmarX.VisionMemory.mem.ltm..configuration:
+# ArmarX.VisionMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.VisionMemory.mem.ltm..configuration = ""
+# ArmarX.VisionMemory.mem.ltm.configuration = {}
-# ArmarX.VisionMemory.mem.ltm..enabled:
+# ArmarX.VisionMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm..enabled = false
-
-
-# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.storagepath = Default value not mapped.
+# ArmarX.VisionMemory.mem.ltm.enabled = false
# ArmarX.VisionMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
index 565bb501..81d7cebf 100644
--- a/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
+++ b/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
@@ -296,7 +296,7 @@
# Attributes:
# - Case sensitivity: yes
# - Required: yes
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName = ::_NOT_SET_::
+ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName = distance_to_obstacle_costmap_provider
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.updatePeriodMs:
diff --git a/scenarios/HumanAwareNavigation/config/example_client.cfg b/scenarios/HumanAwareNavigation/config/example_client.cfg
index eef85301..c21733bd 100644
--- a/scenarios/HumanAwareNavigation/config/example_client.cfg
+++ b/scenarios/HumanAwareNavigation/config/example_client.cfg
@@ -76,171 +76,188 @@
# ArmarX.EnableProfiling = false
-# ArmarX.ExampleClient.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ExampleClient.nav.NavigatorName: No Description
# Attributes:
-# - Default: false
-# - Case sensitivity: yes
+# - Default: navigator
+# - Case sensitivity: no
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleClient.EnableProfiling = false
+ArmarX.ExampleClient.nav.NavigatorName = navigator
-# ArmarX.ExampleClient.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
# Attributes:
-# - Default: Undefined
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.ExampleClient.MinimumLoggingLevel = Undefined
+# ArmarX.LoadLibraries = ""
-# ArmarX.ExampleClient.ObjectName: Name of IceGrid well-known object
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.ObjectName = ""
+# ArmarX.LoggingGroup = ""
-# ArmarX.ExampleClient.mem.robot_state.Memory:
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: RobotState
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mem.robot_state.Memory = RobotState
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
-# ArmarX.ExampleClient.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
# Attributes:
-# - Default: Localization
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mem.robot_state.localizationSegment = Localization
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: true
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled = true
+# ArmarX.StartDebuggerOnCrash = false
-# ArmarX.ExampleClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.ThreadPoolSize = 1
-# ArmarX.ExampleClient.nav.NavigatorName: Name of the Navigator
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
-# - Default: navigator
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.ExampleClient.nav.NavigatorName = navigator
+# ArmarX.TopicSuffix = ""
-# ArmarX.ExampleClient.relativeMovement: The distance between two target poses [mm]
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
-# - Default: 200
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.relativeMovement = 200
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
-# ArmarX.ExampleClient.robotName:
+# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
-# - Default: Armar6
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
+# ArmarX.example_client.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.robotName = Armar6
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.example_client.EnableProfiling = false
-# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# ArmarX.example_client.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: ""
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoadLibraries = ""
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.example_client.MinimumLoggingLevel = Undefined
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# ArmarX.example_client.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoggingGroup = ""
+# ArmarX.example_client.ObjectName = ""
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.example_client.mem.robot_state.Memory:
# Attributes:
-# - Default: true
+# - Default: RobotState
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.example_client.mem.robot_state.Memory = RobotState
-# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# ArmarX.example_client.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
# Attributes:
-# - Default: 3000
+# - Default: Localization
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.example_client.mem.robot_state.localizationSegment = Localization
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.example_client.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
# Attributes:
-# - Default: 0
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.example_client.mns.MemoryNameSystemEnabled = true
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# ArmarX.example_client.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: false
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
+# ArmarX.example_client.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# ArmarX.example_client.mode: Which example to run
# Attributes:
-# - Default: 1
+# - Default: standard
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ThreadPoolSize = 1
+# - Possible values: {complex, point_to_point, standard, update_navigator}
+# ArmarX.example_client.mode = standard
-# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# ArmarX.example_client.nav.NavigatorName: Name of the Navigator
# Attributes:
-# - Default: ""
+# - Default: navigator
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+# ArmarX.example_client.nav.NavigatorName = navigator
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.example_client.relativeMovement: The distance between two target poses [mm]
# Attributes:
-# - Default: false
+# - Default: 200
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
+# ArmarX.example_client.relativeMovement = 200
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.example_client.robotName:
# Attributes:
-# - Default: Info
+# - Default: Armar6
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+# ArmarX.example_client.robotName = Armar6
diff --git a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
index 4fd3110f..a3e73f4f 100644
--- a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
+++ b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
@@ -191,7 +191,7 @@ ArmarX.Verbosity = Verbose
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.navigation_memory.MinimumLoggingLevel = Undefined
+ArmarX.navigation_memory.MinimumLoggingLevel = Verbose
# ArmarX.navigation_memory.ObjectName: Name of IceGrid well-known object
@@ -288,6 +288,3 @@ ArmarX.Verbosity = Verbose
# ArmarX.navigation_memory.p.snapshotToLoad = ""
-# ArmarX.navigation_memory.MinimumLoggingLevel:
-# Attributes:
-ArmarX.navigation_memory.MinimumLoggingLevel = Verbose
diff --git a/scenarios/HumanAwareNavigation/config/navigator.cfg b/scenarios/HumanAwareNavigation/config/navigator.cfg
index 7b44af1c..e55d723f 100644
--- a/scenarios/HumanAwareNavigation/config/navigator.cfg
+++ b/scenarios/HumanAwareNavigation/config/navigator.cfg
@@ -92,46 +92,6 @@
# ArmarX.LoggingGroup = ""
-# ArmarX.Navigator.ObjectName: No Description
-# Attributes:
-# - Default: navigator
-# - Case sensitivity: no
-# - Required: no
-ArmarX.Navigator.ObjectName = navigator
-
-
-# ArmarX.Navigator.RobotUnitName: No Description
-# Attributes:
-# - Default: Armar6Unit
-# - Case sensitivity: no
-# - Required: no
-ArmarX.Navigator.RobotUnitName = Armar6Unit
-
-
-# ArmarX.Navigator.cmp.PlatformUnit: No Description
-# Attributes:
-# - Default: Armar6PlatformUnit
-# - Case sensitivity: no
-# - Required: no
-ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
-
-
-# ArmarX.Navigator.p.disableExecutor: No Description
-# Attributes:
-# - Default: false
-# - Case sensitivity: no
-# - Required: no
-ArmarX.Navigator.p.disableExecutor = false
-
-
-# ArmarX.Navigator.p.occupancy_grid.occopied_threshold: No Description
-# Attributes:
-# - Default: 0.8
-# - Case sensitivity: no
-# - Required: no
-ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
-
-
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: true
@@ -216,6 +176,14 @@ ArmarX.Verbosity = Verbose
# ArmarX.navigator.ArVizTopicName = ArVizTopic
+# ArmarX.navigator.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.DebugObserverTopicName = DebugObserver
+
+
# ArmarX.navigator.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
@@ -254,7 +222,7 @@ ArmarX.Verbosity = Verbose
# Attributes:
# - Case sensitivity: yes
# - Required: yes
-# ArmarX.navigator.RobotUnitName = ::_NOT_SET_::
+ArmarX.navigator.RobotUnitName = Armar6Unit
# ArmarX.navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
@@ -321,6 +289,22 @@ ArmarX.Verbosity = Verbose
# ArmarX.navigator.mem.nav.graph.Memory = Navigation
+# ArmarX.navigator.mem.nav.human.CoreSegment:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.human.CoreSegment = Human
+
+
+# ArmarX.navigator.mem.nav.human.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.human.Memory = Navigation
+
+
# ArmarX.navigator.mem.nav.param.CoreSegment:
# Attributes:
# - Default: Parameterization
@@ -417,6 +401,6 @@ ArmarX.Verbosity = Verbose
# - Default: 0.550000012
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigator.p.occupancy_grid.occopied_threshold = 0.550000012
+ArmarX.navigator.p.occupancy_grid.occopied_threshold = 0.8
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index cc0fcea7..a290831f 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -6,7 +6,7 @@
<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="navigator" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="navigation_memory" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="VisionMemory" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
diff --git a/scenarios/PlatformNavigation/config/ObjectMemory.cfg b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
index b2764c9b..be7ed818 100644
--- a/scenarios/PlatformNavigation/config/ObjectMemory.cfg
+++ b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
@@ -513,21 +513,12 @@
# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
-# ArmarX.ObjectMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# ArmarX.ObjectMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.ltm..buffer.storeFreq = 10
-
-
-# ArmarX.ObjectMemory.mem.ltm.depthImageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.depthImageExtractor.Enabled = true
+# ArmarX.ObjectMemory.mem.ltm.configuration = {}
# ArmarX.ObjectMemory.mem.ltm.enabled:
@@ -539,100 +530,6 @@
# ArmarX.ObjectMemory.mem.ltm.enabled = false
-# ArmarX.ObjectMemory.mem.ltm.exrConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.exrConverter.Enabled = true
-
-
-# ArmarX.ObjectMemory.mem.ltm.imageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.imageExtractor.Enabled = true
-
-
-# ArmarX.ObjectMemory.mem.ltm.memFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.memFreqFilter.Enabled = false
-
-
-# ArmarX.ObjectMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-
-
-# ArmarX.ObjectMemory.mem.ltm.pngConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.pngConverter.Enabled = true
-
-
-# ArmarX.ObjectMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.ObjectMemory.mem.ltm.snapEqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.snapEqFilter.Enabled = false
-
-
-# ArmarX.ObjectMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-
-
-# ArmarX.ObjectMemory.mem.ltm.snapFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.ltm.snapFreqFilter.Enabled = false
-
-
-# ArmarX.ObjectMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
-
-
-# ArmarX.ObjectMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.ObjectMemory.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
@@ -765,3 +662,5 @@
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
index 0dfef493..1b95447f 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -210,21 +210,12 @@
# ArmarX.VisionMemory.mem.MemoryName = Vision
-# ArmarX.VisionMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# ArmarX.VisionMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm..buffer.storeFreq = 10
-
-
-# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled:
-# Attributes:
-# - Default: true
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled = true
+# ArmarX.VisionMemory.mem.ltm.configuration = {}
# ArmarX.VisionMemory.mem.ltm.enabled:
@@ -236,100 +227,6 @@
# ArmarX.VisionMemory.mem.ltm.enabled = false
-# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled = true
-
-
-# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled = true
-
-
-# ArmarX.VisionMemory.mem.ltm.memFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.memFreqFilter.Enabled = false
-
-
-# ArmarX.VisionMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-
-
-# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled = true
-
-
-# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled = false
-
-
-# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-
-
-# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled = false
-
-
-# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
-
-
-# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.VisionMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
@@ -362,3 +259,5 @@
# - Case sensitivity: yes
# - Required: no
# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
+
+
diff --git a/scenarios/PlatformNavigation/config/example_client.cfg b/scenarios/PlatformNavigation/config/example_client.cfg
index 6a9bccd3..f459b030 100644
--- a/scenarios/PlatformNavigation/config/example_client.cfg
+++ b/scenarios/PlatformNavigation/config/example_client.cfg
@@ -76,56 +76,6 @@
# ArmarX.EnableProfiling = false
-# ArmarX.ExampleClient.EnableProfiling: enable profiler which is used for logging performance events
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleClient.EnableProfiling = false
-
-
-# ArmarX.ExampleClient.MinimumLoggingLevel: Local logging level only for this component
-# Attributes:
-# - Default: Undefined
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.ExampleClient.MinimumLoggingLevel = Undefined
-
-
-# ArmarX.ExampleClient.ObjectName: Name of IceGrid well-known object
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleClient.ObjectName = ""
-
-
-# ArmarX.ExampleClient.nav.NavigatorName: Name of the Navigator
-# Attributes:
-# - Default: navigator
-# - Case sensitivity: yes
-# - Required: no
-ArmarX.ExampleClient.nav.NavigatorName = navigator
-
-
-# ArmarX.ExampleClient.relativeMovement: The distance between two target poses [mm]
-# Attributes:
-# - Default: 200
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleClient.relativeMovement = 200
-
-
-# ArmarX.ExampleClient.robotName:
-# Attributes:
-# - Default: Armar6
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleClient.robotName = Armar6
-
-
# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
# Attributes:
# - Default: ""
@@ -208,3 +158,103 @@ ArmarX.ExampleClient.nav.NavigatorName = navigator
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.Verbosity = Info
+
+
+# ArmarX.example_client.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.example_client.EnableProfiling = false
+
+
+# ArmarX.example_client.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.example_client.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.example_client.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.ObjectName = ""
+
+
+# ArmarX.example_client.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.example_client.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.example_client.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.example_client.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.example_client.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.example_client.mode: Which example to run
+# Attributes:
+# - Default: standard
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {complex, point_to_point, standard, update_navigator}
+ArmarX.example_client.mode = standard
+
+
+# ArmarX.example_client.nav.NavigatorName: Name of the Navigator
+# Attributes:
+# - Default: navigator
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.nav.NavigatorName = navigator
+
+
+# ArmarX.example_client.relativeMovement: The distance between two target poses [mm]
+# Attributes:
+# - Default: 200
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.relativeMovement = 200
+
+
+# ArmarX.example_client.robotName:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.robotName = Armar6
+
+
+# ArmarX.ExampleClient.nav.NavigatorName:
+# Attributes:
+ArmarX.ExampleClient.nav.NavigatorName = navigator
+
+
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index 8253a316..74b205e7 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -92,213 +92,204 @@
# ArmarX.LoggingGroup = ""
-# ArmarX.NavigationMemory.ArVizStorageName: Name of the ArViz storage
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: ArVizStorage
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.ArVizStorageName = ArVizStorage
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
-# ArmarX.NavigationMemory.ArVizTopicName: Name of the ArViz topic
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
# Attributes:
-# - Default: ArVizTopic
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.ArVizTopicName = ArVizTopic
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.NavigationMemory.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: false
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.EnableProfiling = false
+# ArmarX.SecondsStartupDelay = 0
-# ArmarX.NavigationMemory.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
# Attributes:
-# - Default: Undefined
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.NavigationMemory.MinimumLoggingLevel = Undefined
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
-# ArmarX.NavigationMemory.ObjectName: Name of IceGrid well-known object
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
-# - Default: ""
+# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.ObjectName = ""
+# ArmarX.ThreadPoolSize = 1
-# ArmarX.NavigationMemory.RemoteGuiName: Name of the remote gui provider
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
-# - Default: RemoteGuiProvider
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.RemoteGuiName = RemoteGuiProvider
+# ArmarX.TopicSuffix = ""
-# ArmarX.NavigationMemory.mem.MemoryName: Name of this memory server.
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
-# - Default: Navigation
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.MemoryName = Navigation
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
-# ArmarX.NavigationMemory.mem.ltm.configuration:
+# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
-# - Default: {}
+# - Default: Info
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm..configuration = ""
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
-# ArmarX.NavigationMemory.mem.ltm.enabled:
+# ArmarX.navigation_memory.ArVizStorageName: Name of the ArViz storage
# Attributes:
-# - Default: false
+# - Default: ArVizStorage
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.mem.ltm..enabled = false
+# ArmarX.navigation_memory.ArVizStorageName = ArVizStorage
-# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
+# ArmarX.navigation_memory.ArVizTopicName: Name of the ArViz topic
# Attributes:
-# - Default: 1024
+# - Default: ArVizTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+# ArmarX.navigation_memory.ArVizTopicName = ArVizTopic
-# ArmarX.NavigationMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
+# ArmarX.navigation_memory.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: Default value not mapped.
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.storagepath = Default value not mapped.
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.EnableProfiling = false
-# ArmarX.NavigationMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.navigation_memory.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: true
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.mns.MemoryNameSystemEnabled = true
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.navigation_memory.MinimumLoggingLevel = Undefined
-# ArmarX.NavigationMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.navigation_memory.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.navigation_memory.ObjectName = ""
-# ArmarX.NavigationMemory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
+# ArmarX.navigation_memory.RemoteGuiName: Name of the remote gui provider
# Attributes:
-# - Default: 2
+# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.p.locationGraph.visuFrequency = 2
+# ArmarX.navigation_memory.RemoteGuiName = RemoteGuiProvider
-# ArmarX.NavigationMemory.p.locationGraph.visuGraphEdges: Enable visualization of navigation graph edges.
+# ArmarX.navigation_memory.mem.MemoryName: Name of this memory server.
# Attributes:
-# - Default: true
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.p.locationGraph.visuGraphEdges = true
+# ArmarX.navigation_memory.mem.MemoryName = Navigation
-# ArmarX.NavigationMemory.p.locationGraph.visuLocation: Enable visualization of locations.
+# ArmarX.navigation_memory.mem.ltm.configuration:
# Attributes:
-# - Default: true
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.NavigationMemory.p.locationGraph.visuLocation = true
+# ArmarX.navigation_memory.mem.ltm.configuration = {}
-# ArmarX.NavigationMemory.p.snapshotToLoad: Memory snapshot to load at start up
-# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').
+# ArmarX.navigation_memory.mem.ltm.enabled:
# Attributes:
-# - Default: ""
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.mem.ltm.enabled = false
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.navigation_memory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.navigation_memory.mns.MemoryNameSystemEnabled = true
-# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# ArmarX.navigation_memory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: 3000
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.navigation_memory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.navigation_memory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
# Attributes:
-# - Default: 0
+# - Default: 2
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.navigation_memory.p.locationGraph.visuFrequency = 2
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# ArmarX.navigation_memory.p.locationGraph.visuGraphEdges: Enable visualization of navigation graph edges.
# Attributes:
-# - Default: false
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
+# ArmarX.navigation_memory.p.locationGraph.visuGraphEdges = true
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# ArmarX.navigation_memory.p.locationGraph.visuLocation: Enable visualization of locations.
# Attributes:
-# - Default: 1
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ThreadPoolSize = 1
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.locationGraph.visuLocation = true
-# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# ArmarX.navigation_memory.p.snapshotToLoad: Memory snapshot to load at start up
+# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+# ArmarX.navigation_memory.p.snapshotToLoad = ""
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.NavigationMemory.p.snapshotToLoad:
# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
+ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
-# ArmarX.Verbosity: Global logging level for whole application
-# Attributes:
-# - Default: Info
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 233f17f4..5bf206ad 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -139,20 +139,12 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.Navigator.ObjectName = navigator
+# ArmarX.TopicSuffix = ""
-# ArmarX.Navigator.RobotUnitName: Name of the RobotUnit
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
-# - Default: Armar6Unit
-# - Case sensitivity: no
-# - Required: no
-ArmarX.Navigator.RobotUnitName = Armar6Unit
-
-
-# ArmarX.Navigator.cmp.PlatformUnit: Ice object name of the `PlatformUnit` component.
-# Attributes:
-# - Default: PlatformUnit
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
@@ -184,6 +176,14 @@ ArmarX.Verbosity = Verbose
# ArmarX.navigator.ArVizTopicName = ArVizTopic
+# ArmarX.navigator.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.DebugObserverTopicName = DebugObserver
+
+
# ArmarX.navigator.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
@@ -278,43 +278,50 @@ ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.nav.stack_result.Provider = ""
+# ArmarX.navigator.mem.nav.events.Provider = ""
-# ArmarX.Navigator.mem.robot_state.Memory:
+# ArmarX.navigator.mem.nav.graph.CoreSegment:
# Attributes:
-# - Default: RobotState
+# - Default: Location
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.robot_state.Memory = RobotState
+# ArmarX.navigator.mem.nav.graph.CoreSegment = Location
-# ArmarX.Navigator.mem.robot_state.descriptionSegment:
+# ArmarX.navigator.mem.nav.graph.Memory:
# Attributes:
-# - Default: Description
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.robot_state.descriptionSegment = Description
+# ArmarX.navigator.mem.nav.graph.Memory = Navigation
-# ArmarX.Navigator.mem.robot_state.localizationSegment:
+# ArmarX.navigator.mem.nav.human.CoreSegment:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.human.CoreSegment = Human
+
+
+# ArmarX.navigator.mem.nav.human.Memory:
# Attributes:
# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigator.mem.nav.graph.Memory = Navigation
+# ArmarX.navigator.mem.nav.human.Memory = Navigation
-# ArmarX.Navigator.mem.robot_state.proprioceptionSegment:
+# ArmarX.navigator.mem.nav.param.CoreSegment:
# Attributes:
-# - Default: Proprioception
+# - Default: Parameterization
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.mem.robot_state.proprioceptionSegment = Proprioception
+# ArmarX.navigator.mem.nav.param.CoreSegment = Parameterization
-# ArmarX.Navigator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.navigator.mem.nav.param.Memory:
# Attributes:
# - Default: Navigation
# - Case sensitivity: yes
@@ -338,15 +345,15 @@ ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.navigator.mem.nav.stack_result.CoreSegment = ""
-# ArmarX.Navigator.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# ArmarX.navigator.mem.nav.stack_result.Memory:
# Attributes:
-# - Default: MemoryUpdates
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Navigator.tpc.sub.MemoryListener = MemoryUpdates
+# ArmarX.navigator.mem.nav.stack_result.Memory = Navigation
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.navigator.mem.nav.stack_result.Provider: Name of this provider
# Attributes:
# - Default: ""
# - Case sensitivity: yes
@@ -403,3 +410,15 @@ ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
# - Case sensitivity: yes
# - Required: no
ArmarX.navigator.p.occupancy_grid.occopied_threshold = 0.8
+
+
+# ArmarX.Navigator.ObjectName:
+# Attributes:
+ArmarX.Navigator.ObjectName = navigator
+
+
+# ArmarX.Navigator.RobotUnitName:
+# Attributes:
+ArmarX.Navigator.RobotUnitName = Armar6Unit
+
+
--
GitLab
From 8ea35a42f6ba63301a644b414adca77ffa86f5cb Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 19 Aug 2022 20:25:49 +0200
Subject: [PATCH 6/7] reming private eigen includes
---
examples/components/example_client/Component.cpp | 3 ---
1 file changed, 3 deletions(-)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 7b48c323..c950667e 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -27,9 +27,6 @@
#include <cmath>
#include <thread>
-#include <Eigen/src/Geometry/AngleAxis.h>
-#include <Eigen/src/Geometry/Translation.h>
-
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/ClockType.h>
--
GitLab
From 1c3ee0eb9377af1b80ccef7ad6438c4fed1e7469 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 19 Aug 2022 20:26:10 +0200
Subject: [PATCH 7/7] reduced verbosity; fixed example component
---
.../components/example_client/Component.cpp | 22 ++++++++++++++-----
.../components/example_client/Component.h | 2 ++
.../navigation/client/ComponentPlugin.cpp | 2 ++
.../navigation/global_planning/AStar.cpp | 11 +++++-----
.../navigation/memory/client/human/Reader.cpp | 5 ++---
source/armarx/navigation/server/Navigator.cpp | 6 ++---
6 files changed, 31 insertions(+), 17 deletions(-)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index c950667e..4bb1c1f7 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -34,6 +34,8 @@
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include "armarx/navigation/core/types.h"
+#include "armarx/navigation/global_planning/SPFA.h"
+#include "armarx/navigation/local_planning/TimedElasticBands.h"
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/types.h>
#include <armarx/navigation/global_planning/AStar.h>
@@ -89,9 +91,17 @@ namespace armarx::navigation::components::example_client
std::string
Component::getDefaultName() const
{
- return "ExampleClient";
+ return GetDefaultName();
}
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return "example_client";
+ }
+
+
void
Component::run()
{
@@ -129,7 +139,8 @@ namespace armarx::navigation::components::example_client
configureNavigator(
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
- .globalPlanner(global_planning::AStarParams())
+ .globalPlanner(global_planning::SPFAParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams()));
// Example of registering a lambda as callback.
@@ -173,7 +184,7 @@ namespace armarx::navigation::components::example_client
configureNavigator(
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
- .globalPlanner(global_planning::AStarParams())
+ .globalPlanner(global_planning::SPFAParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -273,7 +284,7 @@ namespace armarx::navigation::components::example_client
configureNavigator(
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
- .globalPlanner(global_planning::AStarParams())
+ .globalPlanner(global_planning::SPFAParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -444,7 +455,6 @@ namespace armarx::navigation::components::example_client
return def;
}
-
- ARMARX_DECOUPLED_REGISTER_COMPONENT(Component);
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
} // namespace armarx::navigation::components::example_client
diff --git a/examples/components/example_client/Component.h b/examples/components/example_client/Component.h
index 2e622a35..3c6748d3 100644
--- a/examples/components/example_client/Component.h
+++ b/examples/components/example_client/Component.h
@@ -65,6 +65,8 @@ namespace armarx::navigation::components::example_client
~Component() override;
+ static std::string GetDefaultName();
+
protected:
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
diff --git a/source/armarx/navigation/client/ComponentPlugin.cpp b/source/armarx/navigation/client/ComponentPlugin.cpp
index 173e3dd4..2ecd8fce 100644
--- a/source/armarx/navigation/client/ComponentPlugin.cpp
+++ b/source/armarx/navigation/client/ComponentPlugin.cpp
@@ -62,8 +62,10 @@ namespace armarx::navigation::client
eventHandler = [&]() -> std::unique_ptr<SimpleEventHandler>
{
+ ARMARX_TRACE;
if (parentDerives<armarx::armem::client::plugins::ListeningPluginUser>())
{
+ ARMARX_TRACE;
ARMARX_INFO << "Using memory event callbacks.";
// must(!!) use a reference here: otherwise the mns events won't work
auto& memoryNameSystem =
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 67db7790..6765a908 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -283,15 +283,16 @@ namespace armarx::navigation::global_planning
if (not result)
{
- ARMARX_ERROR << "Optimizer failure";
+ ARMARX_WARNING << "Optimizer failure";
return std::nullopt;
}
- // TODO circular path smoothing should be done now
+ ARMARX_TRACE;
- algorithm::CircularPathSmoothing smoothing;
- auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
- smoothTrajectory.setMaxVelocity(params.linearVelocity);
+ // TODO circular path smoothing should be done now
+ // algorithm::CircularPathSmoothing smoothing;
+ // auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
+ // smoothTrajectory.setMaxVelocity(params.linearVelocity);
ARMARX_TRACE;
return GlobalPlannerResult{.trajectory = result.trajectory.value()};
diff --git a/source/armarx/navigation/memory/client/human/Reader.cpp b/source/armarx/navigation/memory/client/human/Reader.cpp
index e3bee9f2..de91ae99 100644
--- a/source/armarx/navigation/memory/client/human/Reader.cpp
+++ b/source/armarx/navigation/memory/client/human/Reader.cpp
@@ -152,8 +152,7 @@ namespace armarx::navigation::memory::client::human
if (not coreSegment.hasProviderSegment(query.providerName))
{
- ARMARX_VERBOSE << "Provider segment `" << query.providerName
- << "` does not exist (yet).";
+ ARMARX_DEBUG << "Provider segment `" << query.providerName << "` does not exist (yet).";
return {.groups = {}, .status = HumanGroupResult::Status::NoData};
}
@@ -162,7 +161,7 @@ namespace armarx::navigation::memory::client::human
if (providerSegment.empty())
{
- ARMARX_VERBOSE << "No entities.";
+ ARMARX_DEBUG << "No entities.";
return {.groups = {},
.status = HumanGroupResult::Status::NoData,
.errorMessage = "No entities"};
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 22913fa5..6c4c51b9 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -804,7 +804,7 @@ namespace armarx::navigation::server
const Duration duration =
armarx::core::time::StopWatch::measure([&]() { updateScene(); });
- ARMARX_VERBOSE << deactivateSpam(0.2)
+ ARMARX_DEBUG << deactivateSpam(0.2)
<< "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]", duration.toMilliSecondsDouble());
@@ -881,7 +881,7 @@ namespace armarx::navigation::server
updateIntrospector(localPlannerResult);
}
});
- ARMARX_VERBOSE << deactivateSpam(0.2)
+ ARMARX_DEBUG << deactivateSpam(0.2)
<< "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]", duration.toMilliSecondsDouble());
@@ -894,7 +894,7 @@ namespace armarx::navigation::server
const Duration duration =
armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
- ARMARX_VERBOSE << deactivateSpam(0.2)
+ ARMARX_DEBUG << deactivateSpam(0.2)
<< "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
srv.debugObserverHelper->setDebugObserverDatafield("monitor update [ms]", duration.toMilliSecondsDouble());
--
GitLab