diff --git a/CMakeLists.txt b/CMakeLists.txt
index 072b8adbc911a27f95d898cc8af9a08b7439018c..49d3cadaf1823b11fe8d2c9b14a641cea2826788 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,7 @@ armarx_project(navigation NAMESPACE armarx)
armarx_find_package(PUBLIC RobotAPI REQUIRED)
# - optional
+armarx_find_package(PUBLIC armarx::control)
armarx_find_package(PUBLIC ArmarXGui)
armarx_find_package(PUBLIC MemoryX QUIET)
armarx_find_package(PUBLIC VisionX QUIET)
diff --git a/data/armarx_navigation/controller_config/PlatformTrajectory/default.json b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
new file mode 100644
index 0000000000000000000000000000000000000000..4cd38813e9ad112a708650aeb00912e426037425
--- /dev/null
+++ b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
@@ -0,0 +1,23 @@
+{
+ "params": {
+ "pidPos": {
+ "Kp": 1,
+ "Ki": 0,
+ "Kd": 0
+ },
+ "pidOri": {
+ "Kp": 1,
+ "Ki": 0,
+ "Kd": 0
+ },
+ "limits": {
+ "linear": 300,
+ "angular": 0.2
+ }
+ },
+ "targets": {
+ "trajectory": {
+ "points": []
+ }
+ }
+}
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 84f1485b2c299ad826d5cca5abd8ff7ebd0a32cc..2900174136483fd185ccd334445783b143c7fef5 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -30,14 +30,14 @@
#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"
-#include "ArmarXCore/core/time/forward_declarations.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/ClockType.h>
+#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/client/types.h>
+#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
@@ -125,8 +125,6 @@ namespace armarx::navigation::components::example_client
getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
- Clock::WaitFor(Duration::Seconds(15));
-
// Wait until goal is reached
armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
if (se)
@@ -292,10 +290,9 @@ namespace armarx::navigation::components::example_client
ARMARX_INFO << "Moving to goal pose";
// Start moving to goal position using above config.
- auto& virtualRobotReader = virtualRobotReaderPlugin->get();
+ auto& virtualRobotReader = virtualRobotReaderPlugin->get();
- auto robot =
- virtualRobotReader.getSynchronizedRobot(properties.robotName, armarx::Clock::Now());
+ auto robot = virtualRobotReader.getSynchronizedRobot(properties.robotName);
ARMARX_CHECK_NOT_NULL(robot);
const core::Pose initialPose(robot->getGlobalPose());
diff --git a/examples/components/example_client/Component.h b/examples/components/example_client/Component.h
index 0fb9bd4cb3a56f26a198d466045f64d461b7a019..295d1964401c7b2025b339e3384340b9a95b37da 100644
--- a/examples/components/example_client/Component.h
+++ b/examples/components/example_client/Component.h
@@ -27,8 +27,8 @@
#include <ArmarXCore/util/tasks.h>
#include <armarx/navigation/client.h>
-#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
-#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
diff --git a/scenarios/NavigationSimulation/NavigationSimulation.scx b/scenarios/NavigationSimulation/NavigationSimulation.scx
index 152187c772cabbd5402fd3c2d344b3a550143ea8..66b83347a8ca47c34f40633555c1207362b0adee 100644
--- a/scenarios/NavigationSimulation/NavigationSimulation.scx
+++ b/scenarios/NavigationSimulation/NavigationSimulation.scx
@@ -45,7 +45,7 @@
<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotStateMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="MotionPlanningServerApp" instance="" package="RobotComponents" nodeName="" enabled="false" iceAutoRestart="false"/>
- <application name="LaserScannerPointCloudProviderApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="LaserScannerPointCloudProviderApp" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="LaserScannerSimulation" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="VisionMemory" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg b/scenarios/NavigationSimulation/config/ArticulatedObjectLocalizerDynamicSimulation.cfg
index a64fd58d4db84713169600f66cfedaa5816951db..7c4574e07410198c26896a48c4f0621b788fface 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/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index b949dad4beef006533842b44ec636c33c31e72ff..57b383ba902a4e4127fea55f55bffe2902169510 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -142,14 +142,6 @@ ArmarX.ObjectMemory.MinimumLoggingLevel = Debug
# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
-# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
-# Attributes:
-# - Default: RobotStateComponent
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
-
-
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
# Attributes:
# - Default: KinematicUnitObserver
@@ -487,6 +479,31 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show: Show arrows linearly predicting object positions.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset: The offset (in seconds) to the current time to make predictions for.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow: The time window (in seconds) into the past to perform the regression on.
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow = 2
+
+
# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels: Prefer articulated object models if available.
# Attributes:
# - Default: true
@@ -616,6 +633,38 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
+# ArmarX.ObjectMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.ObjectMemory.mem.robot_state.descriptionSegment:
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.descriptionSegment = Description
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment:
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment:
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment = Proprioception
+
+
# ArmarX.ObjectMemory.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:
@@ -634,28 +683,28 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003_grasping_challenge_r
# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# ArmarX.ObjectMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
# Attributes:
-# - Default: DebugObserver
+# - Default: 2
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+# ArmarX.ObjectMemory.prediction.TimeWindow = 2
-# ArmarX.ObjectMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# ArmarX.ObjectMemory.robotName:
# Attributes:
-# - Default: MemoryUpdates
+# - Default: Armar6
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.ObjectMemory.robotName = Armar6
-# ArmarX.ObjectMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
-# - Default: MemoryUpdates
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.tpc.sub.MemoryListener = MemoryUpdates
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
diff --git a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
index 5806a1cef862a1d67324a220f894f59219cb4866..71c1ac65747c49cd4ef5e3b45afa18212b631994 100644
--- a/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
+++ b/scenarios/NavigationSimulation/config/RobotStateMemory.cfg
@@ -435,22 +435,6 @@ ArmarX.RobotStateMemory.seg.localization.MaxHistorySize = 3000
ArmarX.RobotStateMemory.seg.proprioception.MaxHistorySize = 3000
-# ArmarX.RobotStateMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotStateMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.RobotStateMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotStateMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
# - Default: 0
diff --git a/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg b/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
index 5bd8e46678f472f111a09cfd84c52b38c7cdabc7..473237552e7086de34df424e4caa3c4c72cbb4b6 100644
--- a/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/RobotUnitSimulationApp.cfg
@@ -363,7 +363,7 @@ ArmarX.RobotUnitSimulation.InverseDynamicsRobotJointSets = PlatformRightArm
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.RobotUnitSimulation.MinimumLoggingLevel = Undefined
+ArmarX.RobotUnitSimulation.MinimumLoggingLevel = Info
# ArmarX.RobotUnitSimulation.NjointController_AllowedExecutionTimePerControlDeviceUntilError: An Error will be printed, If the execution time in micro seconds of a NJointControllerBase exceeds this parameter times the number of ControlDevices.
diff --git a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
index 20d8ea06d78165bda8b5f995d8962aab74f90f00..23b096b32bf457eaea5f3990ff3a0215fb539600 100644
--- a/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
+++ b/scenarios/NavigationSimulation/config/SelfLocalizationDynamicSimulationApp.cfg
@@ -299,14 +299,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/SimulatorApp.cfg b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
index f5557e5ab76c2d9c5e46dc96c6da56210eda3f41..8401b4d00a8c7b13b6ff734105ba9a0ce2539a47 100644
--- a/scenarios/NavigationSimulation/config/SimulatorApp.cfg
+++ b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
@@ -900,7 +900,7 @@ ArmarX.Simulator.SimulationType = kinematics
# - Default: 25
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Simulator.StepTimeMS = 25
+ArmarX.Simulator.StepTimeMS = 10
# ArmarX.Simulator.WorkingMemoryName: Name of WorkingMemory
diff --git a/scenarios/NavigationSimulation/config/VisionMemory.cfg b/scenarios/NavigationSimulation/config/VisionMemory.cfg
index 9c59a9f6c9ab47cdf2a3a1a81ab015c1e6fccfaa..22e0d3e62dbf7e3c0012b44f0d65e2e7a1fe3f46 100644
--- a/scenarios/NavigationSimulation/config/VisionMemory.cfg
+++ b/scenarios/NavigationSimulation/config/VisionMemory.cfg
@@ -364,19 +364,3 @@
# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
-# ArmarX.VisionMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.VisionMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index 76fce00977eacd2a204889de32b54feb0a15c90a..a50c5adc92dab36493e8db75bb95cb40994a61a1 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -6,7 +6,8 @@
<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"/>
</scenario>
diff --git a/scenarios/PlatformNavigation/config/ObjectMemory.cfg b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
index 3c1e0358705c18a37d45cc314687888fd41d0823..579926b6b945c4489f7646549a7a875ea32e5722 100644
--- a/scenarios/PlatformNavigation/config/ObjectMemory.cfg
+++ b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
@@ -142,14 +142,6 @@
# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
-# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
-# Attributes:
-# - Default: RobotStateComponent
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
-
-
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
# Attributes:
# - Default: KinematicUnitObserver
@@ -641,6 +633,38 @@
# ArmarX.ObjectMemory.mem.ltm.storagepath = Default value not mapped.
+# ArmarX.ObjectMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.ObjectMemory.mem.robot_state.descriptionSegment:
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.descriptionSegment = Description
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment:
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment:
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.proprioceptionSegment = Proprioception
+
+
# ArmarX.ObjectMemory.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:
@@ -749,5 +773,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 9c59a9f6c9ab47cdf2a3a1a81ab015c1e6fccfaa..22e0d3e62dbf7e3c0012b44f0d65e2e7a1fe3f46 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -364,19 +364,3 @@
# ArmarX.VisionMemory.pointCloudMaxHistorySize = 20
-# ArmarX.VisionMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.VisionMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.VisionMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
diff --git a/scenarios/PlatformNavigation/config/control_memory.cfg b/scenarios/PlatformNavigation/config/control_memory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0d5c094879b6c331938ae8ac3e138fbd5d8b37d6
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/control_memory.cfg
@@ -0,0 +1,375 @@
+# ==================================================================
+# control_memory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# 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
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.ControlMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.ControlMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ControlMemory.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.ControlMemory.EnableProfiling = false
+
+
+# ArmarX.ControlMemory.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.ControlMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ControlMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.ObjectName = ""
+
+
+# ArmarX.ControlMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ControlMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Control
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.MemoryName = Control
+
+
+# ArmarX.ControlMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm..buffer.storeFreq = 10
+
+
+# ArmarX.ControlMemory.mem.ltm.depthImageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
+# ArmarX.ControlMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ControlMemory.mem.ltm.exrConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.ControlMemory.mem.ltm.imageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.imageExtractor.Enabled = true
+
+
+# ArmarX.ControlMemory.mem.ltm.memFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.memFreqFilter.Enabled = false
+
+
+# ArmarX.ControlMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.memFreqFilter.WaitingTime = -1
+
+
+# ArmarX.ControlMemory.mem.ltm.pngConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.pngConverter.Enabled = true
+
+
+# ArmarX.ControlMemory.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.ControlMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+
+
+# ArmarX.ControlMemory.mem.ltm.snapEqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.snapEqFilter.Enabled = false
+
+
+# ArmarX.ControlMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
+
+
+# ArmarX.ControlMemory.mem.ltm.snapFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ControlMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.ControlMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
+# ArmarX.ControlMemory.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.ControlMemory.mem.ltm.storagepath = Default value not mapped.
+
+
+# ArmarX.ControlMemory.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.ControlMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ControlMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ControlMemory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.p.locationGraph.visuFrequency = 2
+
+
+# ArmarX.ControlMemory.p.snapshotToLoad: Memory snapshot to load at start up
+# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ControlMemory.p.snapshotToLoad = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.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;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# 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.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = 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)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - 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.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# 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: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# 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/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index 604df43db8f358d79669b6faffee43647e91af67..8e727c261d9bc6e67640665b692d20e616000f0c 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -323,22 +323,6 @@
ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
-# ArmarX.NavigationMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.tpc.pub.MemoryListener = MemoryUpdates
-
-
-# ArmarX.NavigationMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
-# Attributes:
-# - Default: MemoryUpdates
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.tpc.sub.MemoryListener = MemoryUpdates
-
-
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: true
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 12e2fb711f0c99c3b841e9f61194a9bb635a22e9..d481cf59b2448853ab66e215e77767ead180e564 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -142,10 +142,17 @@
ArmarX.Navigator.ObjectName = navigator
-# ArmarX.Navigator.cmp.PlatformUnit: Ice object name of the `PlatformUnit` component.
+# ArmarX.Navigator.RobotUnitName: Name of the RobotUnit
# Attributes:
-# - Default: PlatformUnit
# - Case sensitivity: yes
+# - Required: yes
+ArmarX.Navigator.RobotUnitName = Armar6Unit
+
+
+# ArmarX.Navigator.cmp.PlatformUnit: No Description
+# Attributes:
+# - Default: Armar6PlatformUnit
+# - Case sensitivity: no
# - Required: no
ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
@@ -302,22 +309,6 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.Navigator.mem.robot_state.proprioceptionSegment = Proprioception
-# ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment: No Description
-# Attributes:
-# - Default: OccupancyGrid
-# - Case sensitivity: no
-# - Required: no
-ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
-
-
-# ArmarX.Navigator.mem.vision.occupancy_grid.Memory: No Description
-# Attributes:
-# - Default: Vision
-# - Case sensitivity: no
-# - Required: no
-ArmarX.Navigator.mem.vision.occupancy_grid.Memory = Vision
-
-
# 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:
@@ -418,5 +409,3 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
ArmarX.Verbosity = Verbose
-
-
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index 056cdb5a133e2e246bb48d6499f8f81455406d99..9026c247d2c9d9213ca00db9d2895becfca2466c 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/source/armarx/navigation/CMakeLists.txt
@@ -13,6 +13,7 @@ add_subdirectory(factories)
add_subdirectory(location)
add_subdirectory(memory)
add_subdirectory(server)
+add_subdirectory(platform_controller)
# Components.
add_subdirectory(components)
diff --git a/source/armarx/navigation/algorithms/aron_conversions.cpp b/source/armarx/navigation/algorithms/aron_conversions.cpp
index 155ae33296bce7073c845a7c28793fe5d09f020d..031170efbaa9ec93beac9793f508b3578ef1330f 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.cpp
+++ b/source/armarx/navigation/algorithms/aron_conversions.cpp
@@ -1,12 +1,12 @@
#include "aron_conversions.h"
-#include "RobotAPI/libraries/armem/core/wm/memory_definitions.h"
-#include "RobotAPI/libraries/armem/util/util.h"
-#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
-#include "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/conversions/eigen.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/conversions/eigen.h>
namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/aron_conversions.h b/source/armarx/navigation/algorithms/aron_conversions.h
index b595e5bc1d1a56054c4ae97347ce0b891b6f094a..4c91b8330c4f53b4303db407abe53c9fa8f2b5f1 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.h
+++ b/source/armarx/navigation/algorithms/aron_conversions.h
@@ -21,7 +21,7 @@
#pragma once
-#include "RobotAPI/libraries/armem/core/wm/memory_definitions.h"
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
diff --git a/source/armarx/navigation/algorithms/test/algorithms_costmap_test.cpp b/source/armarx/navigation/algorithms/test/algorithms_costmap_test.cpp
index 8b9dd842d02c15485cc407e90b277aed22ec6444..5a721c71b9a38681902afa8444443590f8ce6835 100644
--- a/source/armarx/navigation/algorithms/test/algorithms_costmap_test.cpp
+++ b/source/armarx/navigation/algorithms/test/algorithms_costmap_test.cpp
@@ -22,8 +22,8 @@
*/
-#include "armarx/navigation/algorithms/persistence.h"
-#include "armarx/navigation/algorithms/util.h"
+#include <armarx/navigation/algorithms/persistence.h>
+#include <armarx/navigation/algorithms/util.h>
#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::algorithms
#define ARMARX_BOOST_TEST
diff --git a/source/armarx/navigation/algorithms/util.cpp b/source/armarx/navigation/algorithms/util.cpp
index 2918d94b55b3784c905a4e9eaeec5ed13e95ac7c..fe71db5aeff5c090d9de9148fd366be374cb9158 100644
--- a/source/armarx/navigation/algorithms/util.cpp
+++ b/source/armarx/navigation/algorithms/util.cpp
@@ -39,7 +39,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include "armarx/navigation/algorithms/persistence.h"
+#include <armarx/navigation/algorithms/persistence.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/types.h>
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.cpp b/source/armarx/navigation/client/services/MemorySubscriber.cpp
index 89769e9f6887fb4177c5922fb9f8185fef4479bd..ed515f00207d307adfa068eaf4a52f2dbe451589 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.cpp
+++ b/source/armarx/navigation/client/services/MemorySubscriber.cpp
@@ -2,7 +2,7 @@
#include <mutex>
#include <type_traits>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
@@ -12,7 +12,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include "armarx/navigation/core/aron_conversions.h"
+#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/core/aron/Events.aron.generated.h>
#include <armarx/navigation/core/events.h>
diff --git a/source/armarx/navigation/common/controller_types.h b/source/armarx/navigation/common/controller_types.h
new file mode 100644
index 0000000000000000000000000000000000000000..905ea6def15470f87aa55f44921629db0add69ff
--- /dev/null
+++ b/source/armarx/navigation/common/controller_types.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+namespace armarx::navigation::common
+{
+
+ enum class ControllerType
+ {
+ PlatformTrajectory
+ };
+
+ constexpr const char* PlatformTrajectoryControllerName= "PlatformTrajectory";
+
+ inline const simox::meta::EnumNames<ControllerType> ControllerTypeNames{
+ {ControllerType::PlatformTrajectory, PlatformTrajectoryControllerName}};
+
+} // namespace armarx::navigation::common
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index 9b77ea8e602452727eca0cd71f450115d1b2fefa..c3bec2c61a63b1e21b9b9e6114558e9b013074fe 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -38,8 +38,8 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include "Visu.h"
-#include "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 7090cb6ef73a6303e69290386e6b69b79b0a86ac..3829c503878654192f09a422d670e0e90ecaedaa 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -44,15 +44,15 @@
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "ArmarXCore/core/time/Clock.h"
-#include "ArmarXCore/core/time/Duration.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
@@ -61,6 +61,7 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/interface/ArViz/Elements.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -68,6 +69,7 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include "armarx/navigation/server/scene_provider/SceneProvider.h"
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/astar/util.h>
@@ -84,7 +86,6 @@
#include <armarx/navigation/factories/NavigationStackFactory.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/util/util.h>
@@ -99,7 +100,6 @@ namespace armarx::navigation
std::vector<core::Pose>
convert(const std::vector<Eigen::Matrix4f>& wps)
{
- using namespace armarx::navigation;
std::vector<core::Pose> p;
p.reserve(wps.size());
std::transform(wps.begin(),
@@ -117,15 +117,16 @@ namespace armarx::navigation::components
components::Navigator::Navigator() : parameterizationService(nullptr, nullptr)
// publisher(&resultsWriter, &eventsWriter)
{
- scene.timeServer = &timeServer;
+ // scene().timeServer = &timeServer;
addPlugin(parameterizationReaderPlugin);
addPlugin(parameterizationWriterPlugin);
addPlugin(eventsWriterPlugin);
addPlugin(resultsWriterPlugin);
addPlugin(graphReaderPlugin);
+ addPlugin(costmapReaderPlugin);
addPlugin(costmapWriterPlugin);
-
+
addPlugin(virtualRobotReaderPlugin);
parameterizationService = server::MemoryParameterizationService(
@@ -159,25 +160,34 @@ namespace armarx::navigation::components
virtualRobotReaderPlugin->get().setSyncTimeout(Duration::MilliSeconds(20));
virtualRobotReaderPlugin->get().setSleepAfterSyncFailure(Duration::MilliSeconds(10));
- // initialize scene
+ // initialize scene provider
ARMARX_TRACE;
- initializeScene();
-
- executor = server::PlatformUnitExecutor(platformUnit);
+ {
+ const server::scene_provider::SceneProvider::InjectedServices srv{
+ .graphReader = &graphReaderPlugin->get(),
+ .costmapReader = &costmapReaderPlugin->get(),
+ .virtualRobotReader = &virtualRobotReaderPlugin->get(),
+ .objectPoseClient = ObjectPoseClientPluginUser::getClient()};
+
+ const std::string robotName = getControlComponentPlugin()
+ .getRobotUnitPlugin()
+ .getRobotUnit()
+ ->getKinematicUnit()
+ ->getRobotName();
+
+ const server::scene_provider::SceneProvider::Config cfg{.robotName = robotName};
+ sceneProvider = server::scene_provider::SceneProvider(srv, cfg);
+ }
- introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
+ initializeScene();
- // TODO dynamic scene
- // TODO memory
- // TODO param (10)
+ ARMARX_TRACE;
+ executor.emplace(server::PlatformControllerExecutor(getControlComponentPlugin()));
+ ARMARX_TRACE;
+ introspector = server::ArvizIntrospector(getArvizClient(), scene().robot);
+ // memoryIntrospector = server::MemoryIntrospector(resultsWriterPlugin->get(), );
- robotStateUpdateTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateRobot,
- params.robotUpdatePeriodMs,
- false,
- "RobotStateUpdateTask");
- robotStateUpdateTask->start();
navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
navRemoteGui->enable();
@@ -191,12 +201,6 @@ namespace armarx::navigation::components
components::Navigator::onReconnectComponent()
{
ARMARX_TRACE;
-
- robotStateUpdateTask->start();
-
- // TODO not in all cases meaningful
- //resume();
-
navRemoteGui->enable();
}
@@ -205,10 +209,7 @@ namespace armarx::navigation::components
{
ARMARX_TRACE;
- robotStateUpdateTask->stop();
-
stopAll();
-
navRemoteGui->disable();
}
@@ -217,45 +218,11 @@ namespace armarx::navigation::components
{
}
- VirtualRobot::RobotPtr
- components::Navigator::getRobot()
- {
- ARMARX_TRACE;
- auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
- params.robotName,
- armarx::core::time::Clock::Now(),
- VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
- true);
-
- ARMARX_CHECK_NOT_NULL(robot);
- return robot;
- }
-
void
components::Navigator::initializeScene()
{
ARMARX_TRACE;
-
- scene.robot = getRobot();
-
- {
- std::lock_guard g{scene.staticSceneMtx};
-
- if (not scene.staticScene.has_value())
- {
- scene.staticScene = staticScene();
-
- // cv::Mat1f mat;
- // cv::eigen2cv(scene.staticScene->costmap->getGrid(), mat);
- // cv::imwrite("/tmp/grid.exr", mat);
- }
- }
-
- // TODO dynamic scene
- {
- std::lock_guard g{scene.graphMtx};
- scene.graph = core::SceneGraph{.subgraphs = graphReaderPlugin->get().graphs()};
- }
+ sceneProvider->initialize(armarx::Clock::Now());
}
std::string
@@ -273,9 +240,10 @@ namespace armarx::navigation::components
ARMARX_TRACE;
ARMARX_INFO << "Creating config for caller '" << callerId << "'";
- parameterizationService.store(stackConfig, callerId, timeServer.now());
+ parameterizationService.store(stackConfig, callerId, armarx::Clock::Now());
- server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+ server::NavigationStack stack =
+ fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
@@ -291,12 +259,12 @@ namespace armarx::navigation::components
std::forward_as_tuple(callerId),
std::forward_as_tuple(
server::Navigator::Config{.stack = std::move(stack),
- .scene = &scene,
.general = server::Navigator::Config::General{}},
server::Navigator::InjectedServices{.executor = &executor.value(),
.publisher =
memoryPublishers.at(callerId).get(),
- .introspector = &(introspector.value())}));
+ .introspector = &(introspector.value()),
+ .sceneProvider = &sceneProvider.value()}));
}
void
@@ -344,7 +312,6 @@ namespace armarx::navigation::components
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
- visualizeSPFA();
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
}
@@ -392,8 +359,8 @@ namespace armarx::navigation::components
navigators.at(configId).stop();
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{scene.timeServer->now()},
- core::Pose(scene.robot->getGlobalPose())};
+ const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
+ core::Pose(scene().robot->getGlobalPose())};
memoryPublishers.at(configId)->userAbortTriggered(event);
}
@@ -409,8 +376,8 @@ namespace armarx::navigation::components
}
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{scene.timeServer->now()},
- core::Pose(scene.robot->getGlobalPose())};
+ const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
+ core::Pose(scene().robot->getGlobalPose())};
for (auto& [callerId, memoryPublisher] : memoryPublishers)
{
memoryPublisher->userAbortTriggered(event);
@@ -426,6 +393,13 @@ namespace armarx::navigation::components
return navigators.at(configId).isPaused();
}
+ const core::Scene&
+ Navigator::scene() const
+ {
+ ARMARX_CHECK_NOT_NULL(sceneProvider);
+ return sceneProvider->scene();
+ }
+
bool
components::Navigator::isStopped(const std::string& configId, const Ice::Current&)
{
@@ -449,14 +423,10 @@ namespace armarx::navigation::components
// def->topic<PlatformUnitListener>("MyTopic");
// Use (and depend on) another component (passing the ComponentInterfacePrx).
- def->component(platformUnit);
-
def->component(remoteGui, "RemoteGuiProvider");
// Add a required property.
// def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
- def->optional(params.robotName, "p.robotName");
-
// Add an optionalproperty.
def->optional(params.occupiedGridThreshold,
"p.occupancy_grid.occopied_threshold",
@@ -507,162 +477,6 @@ namespace armarx::navigation::components
arviz.commit(layer);
}
- core::StaticScene
- components::Navigator::staticScene()
- {
- ARMARX_TRACE;
-
- const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
-
- // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
- const auto objectPosesStatic =
- armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
-
- const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
- ARMARX_INFO << objects->getSize() << " objects in the scene";
-
- ARMARX_INFO << "Creating costmap";
- ARMARX_CHECK_NOT_NULL(scene.robot);
-
- algorithms::CostmapBuilder costmapBuilder(
- scene.robot,
- objects,
- algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- "Platform-navigation-colmodel");
-
- const auto costmap = costmapBuilder.create();
-
- ARMARX_INFO << "Storing costmap in memory";
- costmapWriterPlugin->get().store(
- costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
-
- ARMARX_INFO << "Done";
-
- ARMARX_TRACE;
- const auto grid = costmap.getGrid();
-
- core::StaticScene scene{.objects = objects,
- .costmap = std::make_unique<algorithms::Costmap>(costmap)};
-
- ARMARX_INFO << "The object scene consists of " << scene.objects->getSize() << " objects";
-
-
- visualize(costmap, arviz, "distance");
-
- /*
- const armem::vision::occupancy_grid::client::Reader::Result result =
- occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
- .providerName = "CartographerMappingAndLocalization",
- .timestamp = armem::Time::Now()});
-
- if (result and result.occupancyGrid.has_value() and false) // Feature disabled on master ...
- {
- ARMARX_INFO << "Occupancy grid available!";
-
- const auto occupancyGridSceneElements = util::asSceneObjects(
- result.occupancyGrid.value(),
- OccupancyGridHelper::Params{.freespaceThreshold = 0.45F,
- .occupiedThreshold = params.occupiedGridThreshold});
- ARMARX_INFO << occupancyGridSceneElements->getSize()
- << " scene elements from occupancy grid";
-
- scene.objects->addSceneObjects(occupancyGridSceneElements);
-
- // draw
- auto layer = arviz.layer("occupancy_grid");
-
- for (const auto& sceneObject : occupancyGridSceneElements->getSceneObjects())
- {
- const Eigen::Isometry3f world_T_obj(sceneObject->getGlobalPose());
- ARMARX_INFO << world_T_obj.translation();
- ARMARX_INFO << layer.size();
- layer.add(viz::Box("box_" + std::to_string(layer.size()))
- .pose(world_T_obj)
- .size(result.occupancyGrid->resolution)
- .color(viz::Color::orange()));
- }
-
- ARMARX_INFO << "Creating costmap";
-
- algorithms::CostmapBuilder costmapBuilder(
- getRobot(),
- scene.objects,
- algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- "Platform-navigation-colmodel");
-
- const auto costmap = costmapBuilder.create();
-
- ARMARX_INFO << "Done";
-
- ARMARX_TRACE;
- ARMARX_INFO << "Saving costmap.";
- algorithms::save(costmap, "/tmp/navigation-costmap");
-
- arviz.commit({layer});
- }
- else
- {
- ARMARX_INFO << "Occupancy grid not available";
- }*/
-
- return scene;
- }
-
- void
- components::Navigator::visualizeSPFA()
- {
- algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams{
- .obstacleDistanceCosts = true};
-
- ARMARX_INFO << "spfa...";
- algorithms::spfa::ShortestPathFasterAlgorithm spfa(*scene.staticScene->costmap, spfaParams);
- const auto result123 = spfa.spfa(getRobot()->getGlobalPosition().head<2>());
- ARMARX_INFO << "spfa done...";
-
- const auto sceneBounds = algorithms::computeSceneBounds(scene.staticScene->objects);
-
- ARMARX_INFO << "Costmap SPFA";
- algorithms::Costmap cm(result123.distances, algorithms::Costmap::Parameters{}, sceneBounds);
- visualize(cm, arviz, "spfa");
- ARMARX_INFO << "Done.";
- }
-
-
- void
- components::Navigator::updateRobot()
- {
- const auto timestamp = armem::Time::Now();
-
- auto& virtualRobotReader = virtualRobotReaderPlugin->get();
-
- ARMARX_TRACE;
- ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*scene.robot, timestamp));
-
- ARMARX_TRACE;
- const auto robotDescription =
- virtualRobotReader.queryDescription(params.robotName, timestamp);
- ARMARX_CHECK(robotDescription.has_value());
-
- // synchronizing platform state
- ARMARX_TRACE;
- const std::optional<armem::robot::PlatformState> platformState =
- virtualRobotReader.queryPlatformState(robotDescription.value(), armem::Time::Now());
- ARMARX_CHECK(platformState.has_value());
-
- // TODO / FIXME make this thread safe!
- {
- std::lock_guard g{scene.platformVelocityMtx};
-
- if (not scene.platformVelocity.has_value())
- {
- scene.platformVelocity = core::Twist();
- }
-
- scene.platformVelocity->linear = platformState->twist.linear;
- scene.platformVelocity->angular = platformState->twist.angular;
- }
- }
-
server::Navigator*
components::Navigator::activeNavigator()
{
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 6fa6459d03cf26b18d4735adff071787ed6e7ec7..7c725c3197c3774980dabdfda27115b5cf5db64d 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -35,34 +35,36 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
-#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
-#include "armarx/navigation/memory/client/costmap/Writer.h"
+#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/components/Navigator/RemoteGui.h>
-#include <armarx/navigation/core/time/ChronoMonotonicTimeServer.h>
#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/memory/client/costmap/Writer.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/memory/client/parameterization/Reader.h>
#include <armarx/navigation/memory/client/stack_result/Writer.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
+#include <armarx/navigation/server/execution/PlatformControllerExecutor.h>
#include <armarx/navigation/server/introspection/ArvizIntrospector.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/server/parameterization/MemoryParameterizationService.h>
+#include <armarx/navigation/server/scene_provider/SceneProvider.h>
namespace armarx::navigation::components
@@ -83,8 +85,9 @@ namespace armarx::navigation::components
virtual public Component,
virtual public client::NavigatorInterface,
virtual public ObjectPoseClientPluginUser,
- virtual public ArVizComponentPluginUser
- // virtual public armem::ListeningClientPluginUser
+ virtual public ArVizComponentPluginUser,
+ virtual public armarx::control::client::ComponentPluginUser
+ // virtual public armem::ListeningClientPluginUser
{
public:
@@ -128,11 +131,7 @@ namespace armarx::navigation::components
bool isStopped(const std::string& callerId,
const Ice::Current& c = Ice::emptyCurrent) override;
- const core::Scene&
- getScene() const
- {
- return scene;
- }
+ const core::Scene& scene() const;
protected:
/// @see PropertyUser::createPropertyDefinitions()
@@ -144,42 +143,38 @@ namespace armarx::navigation::components
/// @see armarx::ManagedIceObject::onConnectComponent()
void onConnectComponent() override;
- void onReconnectComponent();
-
/// @see armarx::ManagedIceObject::onDisconnectComponent()
void onDisconnectComponent() override;
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
+ void onReconnectComponent();
void initializeScene();
- core::StaticScene staticScene();
+ // core::StaticScene staticScene();
- [[nodiscard]] VirtualRobot::RobotPtr getRobot();
- void updateRobot();
+ // [[nodiscard]] VirtualRobot::RobotPtr getRobot();
+ // void updateRobot();
server::Navigator* activeNavigator();
private:
void visualizeSPFA();
- // TODO update context periodically
-
- // TODO: Remove dependency to PlatformUnit. This component is generally
- // capable to also control drones or the like...
- PlatformUnitInterfacePrx platformUnit;
RemoteGuiInterfacePrx remoteGui;
- core::Scene scene;
- std::optional<server::PlatformUnitExecutor> executor;
+ // core::Scene scene;
+
+ std::optional<server::PlatformControllerExecutor> executor;
std::optional<server::ArvizIntrospector> introspector;
+ // std::optional<server::MemoryIntrospector> memoryIntrospector;
+ std::optional<server::scene_provider::SceneProvider> sceneProvider;
+
std::unordered_map<std::string, server::Navigator> navigators;
std::mutex propertiesMutex;
- PeriodicTask<Navigator>::pointer_type robotStateUpdateTask;
-
// TODO maybe as optional, but requires some effort
std::unique_ptr<NavigatorRemoteGui> navRemoteGui;
@@ -188,17 +183,26 @@ namespace armarx::navigation::components
std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
// `navigation` memory reader and writer
- armem::client::plugins::ReaderWriterPlugin<memory::client::param::Reader>* parameterizationReaderPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::param::Writer>* parameterizationWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::events::Writer>* eventsWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::stack_result::Writer>* resultsWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::graph::Reader>* graphReaderPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>* costmapWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::param::Reader>*
+ parameterizationReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::param::Writer>*
+ parameterizationWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::events::Writer>*
+ eventsWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::stack_result::Writer>*
+ resultsWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::graph::Reader>*
+ graphReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Reader>*
+ costmapReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>*
+ costmapWriterPlugin = nullptr;
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
// `robot_state` memory reader and writer
std::optional<armem::robot::RobotDescription> robotDescription;
- armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin = nullptr;
server::MemoryParameterizationService parameterizationService;
@@ -207,16 +211,9 @@ namespace armarx::navigation::components
// key is `callerId`
std::unordered_map<std::string, std::unique_ptr<server::MemoryPublisher>> memoryPublishers;
- core::ChronoMonotonicTimeServer timeServer;
-
-
struct Parameters
{
float occupiedGridThreshold{0.55F};
-
- std::string robotName = "Armar6";
-
- int robotUpdatePeriodMs = 10;
};
Parameters params;
diff --git a/source/armarx/navigation/components/Navigator/RemoteGui.cpp b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
index 2f8afd275ed36e8b8b850f0ba2a20ec9bce09ade..9ab923ba04998de737a9513b59e789ee3bc33b4a 100644
--- a/source/armarx/navigation/components/Navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
@@ -16,14 +16,13 @@
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
#include "Navigator.h"
-#include "armarx/navigation/global_planning/SPFA.h"
+#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/server/Navigator.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
#include <armarx/navigation/util/util.h>
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index 9b03d6b0d2e8c9e9d8471b19bf6e972725780fbc..fa926fb76b98f3f617ba928d62f16c072d477713 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -33,19 +33,19 @@
#include <SimoxUtility/algorithm/vector.hpp>
#include <SimoxUtility/color/cmaps/colormaps.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
-#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-#include "RobotAPI/interface/ArViz/Elements.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
-#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
-#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
+#include <RobotAPI/interface/ArViz/Elements.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h>
-#include "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h"
-#include "armarx/navigation/memory/client/costmap/Reader.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/conversions/eigen.h>
// Include headers you only need in function definitions in the .cpp.
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
index 7f4f9ba4b3ce817a95e6d5c876e342506625db33..6094a09eb1b1b84849c7677c029837d43f8c8bba 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -23,17 +23,17 @@
#pragma once
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
-#include "RobotAPI/libraries/armem/client/forward_declarations.h"
-#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
-#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client.h>
-#include "armarx/navigation/memory/client/costmap/Reader.h"
-#include "armarx/navigation/memory/client/costmap/Writer.h"
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/costmap/Writer.h>
#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
diff --git a/source/armarx/navigation/core/CMakeLists.txt b/source/armarx/navigation/core/CMakeLists.txt
index 6b56cc432b6266039208ad37d552ef39c39d6e5c..4ee7e461279c24d83a67e7739b2df3f17ae1a822 100644
--- a/source/armarx/navigation/core/CMakeLists.txt
+++ b/source/armarx/navigation/core/CMakeLists.txt
@@ -17,7 +17,6 @@ armarx_add_library(core
Graph.cpp
aron_conversions.cpp
json_conversions.cpp
- time/ChronoMonotonicTimeServer.cpp
HEADERS
Trajectory.h
constants.h
@@ -35,8 +34,6 @@ armarx_add_library(core
TopologicScene.h
aron_conversions.h
json_conversions.h
- time/ChronoMonotonicTimeServer.h
- time/TimeServerInterface.h
DEPENDENCIES_PUBLIC
ArmarXCoreInterfaces
ArmarXCore
diff --git a/source/armarx/navigation/core/DynamicScene.h b/source/armarx/navigation/core/DynamicScene.h
index 1639a72657ae64a35ca34ecdcd847c293a7c60b5..05f211a9a6a4d40e243ab560a437814d3eb02b94 100644
--- a/source/armarx/navigation/core/DynamicScene.h
+++ b/source/armarx/navigation/core/DynamicScene.h
@@ -22,16 +22,18 @@
#pragma once
-#include <memory>
+
namespace armarx::navigation::core
{
- class DynamicScene
+ struct DynamicScene
{
+
+ // TODO(SALt): Implement
+
public:
protected:
private:
};
- using DynamicScenePtr = std::shared_ptr<DynamicScene>;
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/StaticScene.h b/source/armarx/navigation/core/StaticScene.h
index 1f4ad33aae03bc2da57da3cec8cb45d003a205b9..de0f008db6a521816bebb17d3534ba1903efc3f3 100644
--- a/source/armarx/navigation/core/StaticScene.h
+++ b/source/armarx/navigation/core/StaticScene.h
@@ -34,9 +34,11 @@ namespace armarx::navigation::core
struct StaticScene
{
VirtualRobot::SceneObjectSetPtr objects;
+
+ // TODO(fabian.reister): rename, why unique_ptr
std::unique_ptr<algorithms::Costmap> costmap;
+
};
- using StaticScenePtr = std::shared_ptr<StaticScene>;
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index e9985faf1509d05a14ab502a2e728805a041b65e..fd5f7e4f7d0def4e14d97ffbb6d69a09d047bf4d 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -66,6 +66,8 @@ namespace armarx::navigation::core
class Trajectory
{
public:
+ Trajectory() = default;
+
Trajectory(const std::vector<TrajectoryPoint>& points) : pts(points)
{
}
diff --git a/source/armarx/navigation/core/aron_conversions.cpp b/source/armarx/navigation/core/aron_conversions.cpp
index 0aeaa75b3a936246a3a700348d4733f96478dff1..7b93cdc279833f1cd8f2467cd6950c98a73a8c38 100644
--- a/source/armarx/navigation/core/aron_conversions.cpp
+++ b/source/armarx/navigation/core/aron_conversions.cpp
@@ -3,7 +3,7 @@
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
-#include "RobotAPI/libraries/aron/common/aron_conversions/core.h"
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/Trajectory.h>
diff --git a/source/armarx/navigation/core/json_conversions.cpp b/source/armarx/navigation/core/json_conversions.cpp
index 21b6cd17910f7c37853c3ac7b1c5514534315cae..307b8bde41ebbe4ce91023fcb712ab83fa1ef95e 100644
--- a/source/armarx/navigation/core/json_conversions.cpp
+++ b/source/armarx/navigation/core/json_conversions.cpp
@@ -20,28 +20,3 @@
*/
#include "json_conversions.h"
-
-#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-
-namespace armarx::navigation::core
-{
-
- void
- arondto::to_json(nlohmann::json& j, const Edge& bo)
- {
- armarx::aron::data::writer::NlohmannJSONWriter writer;
- j = bo.write(writer);
- }
-
-
- void
- arondto::from_json(const nlohmann::json& j, Edge& bo)
- {
- armarx::aron::data::reader::NlohmannJSONReader reader;
- bo.read(reader, j);
- }
-
-
-} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/json_conversions.h b/source/armarx/navigation/core/json_conversions.h
index a8fd2caac4355d8eea6898050bb4cccea04626de..7be22d49d7dfb0a537fd98f7cfbf0f92919300e5 100644
--- a/source/armarx/navigation/core/json_conversions.h
+++ b/source/armarx/navigation/core/json_conversions.h
@@ -23,14 +23,33 @@
#include <SimoxUtility/json/json.hpp>
-#include <armarx/navigation/core/aron/Graph.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronGeneratedClass.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-namespace armarx::navigation::core::arondto
+namespace armarx
{
-
- void to_json(nlohmann::json& j, const arondto::Edge& bo);
- void from_json(const nlohmann::json& j, arondto::Edge& bo);
-
-
-} // namespace armarx::navigation::core::arondto
+ template <typename AronDTO>
+ void
+ from_json(const nlohmann::json& j, AronDTO& dto)
+ {
+ static_assert(
+ std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronDTO>::value);
+
+ armarx::aron::data::reader::NlohmannJSONReader reader;
+ dto.read(reader, j);
+ }
+
+
+ template <typename AronDTO>
+ void
+ to_json(nlohmann::json& j, const AronDTO& bo)
+ {
+ static_assert(
+ std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronDTO>::value);
+
+ armarx::aron::data::writer::NlohmannJSONWriter writer;
+ j = bo.write(writer);
+ }
+
+} // namespace armarx
diff --git a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp b/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp
deleted file mode 100644
index fb3db7cb1dc0f96915ed982c820690a6fc5ad1e1..0000000000000000000000000000000000000000
--- a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "ChronoMonotonicTimeServer.h"
-
-#include <ArmarXCore/core/time/Clock.h>
-
-namespace armarx::navigation::core
-{
- armarx::core::time::DateTime
- ChronoMonotonicTimeServer::now() const
- {
- return armarx::Clock::Now();
- }
-
-} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index a0529cb0f42b37215936a30fd3038d486ab80492..7d338dfd201520aeef8a567d9427fc932503d09b 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -78,21 +78,14 @@ namespace armarx::navigation::core
struct Scene
{
- mutable std::mutex staticSceneMtx;
- std::optional<core::StaticScene> staticScene = std::nullopt;
+ DateTime timestamp = DateTime::Invalid();
- mutable std::mutex dynamicSceneMtx;
+ std::optional<core::StaticScene> staticScene = std::nullopt;
std::optional<core::DynamicScene> dynamicScene = std::nullopt;
- // TopologicMapPtr topologicMap;
- VirtualRobot::RobotPtr robot;
- mutable std::mutex platformVelocityMtx;
- std::optional<core::Twist> platformVelocity;
+ VirtualRobot::RobotPtr robot;
- mutable std::mutex graphMtx;
std::optional<core::SceneGraph> graph;
-
- TimeServerInterface* timeServer;
};
struct PIDParams
diff --git a/source/armarx/navigation/factories/NavigationStackFactory.cpp b/source/armarx/navigation/factories/NavigationStackFactory.cpp
index 68fa77a0a528707e127364950bf1ba1245d0a734..9869c6a707f9a12a890492d8f6388763f9ce7cec 100644
--- a/source/armarx/navigation/factories/NavigationStackFactory.cpp
+++ b/source/armarx/navigation/factories/NavigationStackFactory.cpp
@@ -40,9 +40,9 @@ namespace armarx::navigation::fac
return server::NavigationStack{
.globalPlanner = GlobalPlannerFactory::create(globalPlannerCfg, ctx),
- .localPlanner = LocalPlannerFactory::create(localPlannerCfg, ctx),
- .trajectoryControl = TrajectoryControllerFactory::create(trajectoryControllerCfg, ctx),
- .safetyControl = SafetyControllerFactory::create(safetyControllerCfg, ctx)};
+ .localPlanner = LocalPlannerFactory::create(localPlannerCfg, ctx)};
+ // .trajectoryControl = TrajectoryControllerFactory::create(trajectoryControllerCfg, ctx),
+ // .safetyControl = SafetyControllerFactory::create(safetyControllerCfg, ctx)};
}
server::NavigationStack
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
index 277d277deaffc3d019d93ebeb027c3451573ca0c..3b8f6a392c37c4a45dfcaf873564158c138c92a2 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
@@ -12,7 +12,8 @@
namespace armarx::navigation::fac
{
traj_ctrl::TrajectoryControllerPtr
- TrajectoryControllerFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
+ TrajectoryControllerFactory::create(const VirtualRobot::RobotPtr& robot,
+ const aron::data::DictPtr& params)
{
namespace layer = traj_ctrl;
@@ -35,11 +36,11 @@ namespace armarx::navigation::fac
{
case traj_ctrl::Algorithms::WaypointController:
controller = std::make_shared<traj_ctrl::WaypointController>(
- traj_ctrl::WaypointControllerParams::FromAron(algoParams), ctx);
+ traj_ctrl::WaypointControllerParams::FromAron(algoParams));
break;
case traj_ctrl::Algorithms::TrajectoryFollowingController:
controller = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
- traj_ctrl::TrajectoryFollowingControllerParams::FromAron(algoParams), ctx);
+ robot, traj_ctrl::TrajectoryFollowingControllerParams::FromAron(algoParams));
break;
}
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.h b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
index 16328e1c76889c6fbdefbab975baf9b4a40bb5a4..cd35d86046979841b263ad183b87fefdf6ec9643 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.h
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
@@ -23,6 +23,7 @@
#pragma once
// #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
#include <armarx/navigation/core/fwd.h>
@@ -38,8 +39,8 @@ namespace armarx::navigation::fac
class TrajectoryControllerFactory
{
public:
- static traj_ctrl::TrajectoryControllerPtr create(const aron::data::DictPtr& params,
- const core::Scene& ctx);
+ static traj_ctrl::TrajectoryControllerPtr create(const VirtualRobot::RobotPtr& robot,
+ const aron::data::DictPtr& params);
protected:
private:
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index fdac1e34026f54eea859c6da61241f9e960903ab..3dcd307cdeb00462033d892afbeadebf1556b479 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -158,7 +158,6 @@ namespace armarx::navigation::global_planning
{
ARMARX_TRACE;
- std::lock_guard g{scene.staticSceneMtx};
// FIXME check if costmap is available
algorithm::astar::AStarPlanner planner(*scene.staticScene->costmap);
diff --git a/source/armarx/navigation/global_planning/GlobalPlanner.h b/source/armarx/navigation/global_planning/GlobalPlanner.h
index 8298361991b4235638517a83225183ac39d62f6b..1a028db1566ea96bf8c2a4f9ff060e9e9e8f0e9c 100644
--- a/source/armarx/navigation/global_planning/GlobalPlanner.h
+++ b/source/armarx/navigation/global_planning/GlobalPlanner.h
@@ -42,9 +42,9 @@ namespace armarx::navigation::global_planning
/**
- * @brief Parameters for GlobalPlanner
- *
- */
+ * @brief Parameters for GlobalPlanner
+ *
+ */
struct GlobalPlannerParams
{
bool foo;
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
index b5351314585aa63f84d05ffa15f8cf40b30616b6..e5c1c7cd5e120126e86c16a3af3449f20419d476 100644
--- a/source/armarx/navigation/global_planning/SPFA.cpp
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -88,7 +88,6 @@ namespace armarx::navigation::global_planning
{
ARMARX_TRACE;
- std::lock_guard g{scene.staticSceneMtx};
// FIXME check if costmap is available
algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams;
diff --git a/source/armarx/navigation/graph/constants.h b/source/armarx/navigation/graph/constants.h
index 011c0c53cd8fcda495f49b9340862c3b2d7c58ca..2676cc6d725ff9dac8bce01af28769ecbed60f8f 100644
--- a/source/armarx/navigation/graph/constants.h
+++ b/source/armarx/navigation/graph/constants.h
@@ -23,7 +23,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::graph
{
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index e43747cdaca8c715d021a61ad8d534de7714d677..8b965d154ad4a997ff530f5adf73083386461dc1 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -24,16 +24,19 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/local_planning/core.h>
namespace armarx::navigation::loc_plan
{
+ // TODO(SALt): Implement
struct TimedElasticBandsParams : public LocalPlannerParams
{
bool includeStartPose{false};
+
Algorithms algorithm() const override;
aron::data::DictPtr toAron() const override;
static TimedElasticBandsParams FromAron(const aron::data::DictPtr& dict);
@@ -47,6 +50,8 @@ namespace armarx::navigation::loc_plan
TimedElasticBands(const Params& params, const core::Scene& ctx);
~TimedElasticBands() override = default;
+ void init(const core::Trajectory& initialTrajectory);
+
std::optional<LocalPlannerResult> plan(const core::Trajectory& goal) override;
protected:
diff --git a/source/armarx/navigation/location/constants.h b/source/armarx/navigation/location/constants.h
index 5211f27cf66010aa0a5311c069748570b0f387e0..5e138e1e0290865dece577451b151106cc571a92 100644
--- a/source/armarx/navigation/location/constants.h
+++ b/source/armarx/navigation/location/constants.h
@@ -23,7 +23,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::location
{
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.cpp b/source/armarx/navigation/memory/client/costmap/Reader.cpp
index 17a48dbcd66bb523492bc885c6ded10251f60b52..436fd2b9462f5a3132607f7302315443ccb4e5eb 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Reader.cpp
@@ -16,9 +16,9 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include "armarx/navigation/algorithms/Costmap.h"
-#include "armarx/navigation/algorithms/aron_conversions.h"
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::costmap
{
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.h b/source/armarx/navigation/memory/client/costmap/Reader.h
index 24b02adab251453551dbe579afe18e0d08933d21..e970a5f246325fe2e80c18dfbd9fadec06768504 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.h
+++ b/source/armarx/navigation/memory/client/costmap/Reader.h
@@ -26,7 +26,7 @@
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include "armarx/navigation/algorithms/Costmap.h"
+#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::memory::client::costmap
{
diff --git a/source/armarx/navigation/memory/client/costmap/Writer.cpp b/source/armarx/navigation/memory/client/costmap/Writer.cpp
index f076fe1ceb1dbec0b370c9a1207697e4b476eb4c..9fcca3947fd9a51c261c3f6d4941c9dccc743215 100644
--- a/source/armarx/navigation/memory/client/costmap/Writer.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Writer.cpp
@@ -1,8 +1,8 @@
#include "Writer.h"
-#include "armarx/navigation/algorithms/aron/Costmap.aron.generated.h"
-#include "armarx/navigation/algorithms/aron_conversions.h"
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::costmap
diff --git a/source/armarx/navigation/memory/client/costmap/Writer.h b/source/armarx/navigation/memory/client/costmap/Writer.h
index 0e44e30d886e17a7d218ecb32d78d08e9724fafe..f80cdc1a168529b1628c7475912b287519113efe 100644
--- a/source/armarx/navigation/memory/client/costmap/Writer.h
+++ b/source/armarx/navigation/memory/client/costmap/Writer.h
@@ -26,7 +26,7 @@
#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
#include <RobotAPI/libraries/armem_vision/types.h>
-#include "armarx/navigation/algorithms/Costmap.h"
+#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::memory::client::costmap
{
diff --git a/source/armarx/navigation/memory/client/events/Writer.cpp b/source/armarx/navigation/memory/client/events/Writer.cpp
index 182209501e810a9c3e62709764501743674eaa45..83009655f02b687e2a00763a0e275a5f56bd8e17 100644
--- a/source/armarx/navigation/memory/client/events/Writer.cpp
+++ b/source/armarx/navigation/memory/client/events/Writer.cpp
@@ -5,7 +5,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/core/aron/Events.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/core/constants.h>
diff --git a/source/armarx/navigation/memory/client/parameterization/Reader.cpp b/source/armarx/navigation/memory/client/parameterization/Reader.cpp
index 311a5f4b771b6c22e1b22965992bb4da9f1f1145..e050b32557599ca74cf23b137bf23255352ffddb 100644
--- a/source/armarx/navigation/memory/client/parameterization/Reader.cpp
+++ b/source/armarx/navigation/memory/client/parameterization/Reader.cpp
@@ -1,6 +1,6 @@
#include "Reader.h"
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::param
{
diff --git a/source/armarx/navigation/platform_controller/CMakeLists.txt b/source/armarx/navigation/platform_controller/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..509c8f9af074a17329a3a9ba0282f7a411370dcd
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/CMakeLists.txt
@@ -0,0 +1,24 @@
+armarx_add_aron_library(platform_controller_aron
+ ARON_FILES
+ aron/PlatformTrajectoryControllerConfig.xml
+ DEPENDENCIES
+ armarx_control::common_aron
+)
+
+armarx_add_library(platform_controller
+ SOURCES
+ # WholeBodyImpedanceController.cpp
+ PlatformTrajectoryController.cpp
+ aron_conversions.cpp
+ HEADERS
+ # WholeBodyImpedanceController.h
+ PlatformTrajectoryController.h
+ aron_conversions.h
+ DEPENDENCIES_PUBLIC
+ Simox::VirtualRobot
+ armarx_control::common
+ armarx_control::client
+ armarx_navigation::core
+ armarx_navigation::trajectory_control
+ # armarx_control::njoint_qp_controller_aron
+)
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5da2a9095d386ece1ec7189a31e7a4c5ef04efc
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp
@@ -0,0 +1,169 @@
+#include "PlatformTrajectoryController.h"
+
+#include "ArmarXCore/core/ArmarXObjectScheduler.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/CycleUtil.h"
+
+#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h"
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
+
+#include <armarx/control/common/aron_conversions.h>
+#include <armarx/control/common/type.h>
+// #include <armarx/control/common/utils.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron_conversions.h>
+
+namespace armarx::navigation::platform_controller::platform_trajectory
+{
+ const NJointControllerRegistration<Controller> Registration(
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory));
+
+ Controller::Controller(const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_IMPORTANT << "Creating "
+ << common::ControllerTypeNames.to_name(
+ common::ControllerType::PlatformTrajectory);
+ // config
+ ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(cfg);
+ ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
+
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+
+ const auto robot = useSynchronizedRtRobot();
+
+ platformTarget = useControlTarget(robotUnit->getRobotPlatformName(),
+ ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(platformTarget)
+ << "The actuator " << robotUnit->getRobotPlatformName() << " has no control mode "
+ << ControlModes::HolonomicPlatformVelocity;
+
+ const auto configData = ::armarx::fromAron<arondto::Config, Config>(cfg->config);
+ const auto trajectoryFollowingControllerParams = configData.params;
+
+ reinitTripleBuffer({});
+ configBuffer.reinitAllBuffers(configData);
+
+ trajectoryFollowingController.emplace(robot, trajectoryFollowingControllerParams);
+
+ ARMARX_INFO << "Init done.";
+ }
+
+ std::string
+ Controller::getClassName(const Ice::Current& iceCurrent) const
+ {
+ return armarx::navigation::common::ControllerTypeNames.to_name(
+ armarx::navigation::common::ControllerType::PlatformTrajectory);
+ }
+
+ void
+ Controller::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtUpdateControlStruct();
+
+ platformTarget->velocityX = rtGetControlStruct().platformVelocityTargets.x;
+ platformTarget->velocityY = rtGetControlStruct().platformVelocityTargets.y;
+ platformTarget->velocityRotation = rtGetControlStruct().platformVelocityTargets.yaw;
+ }
+
+ void
+ Controller::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
+ const Ice::Current& iceCurrent)
+ {
+ ARMARX_IMPORTANT << "Controller::updateConfig";
+
+ // TODO maybe update pid controller
+
+ auto updateConfig = ::armarx::fromAron<arondto::Config, Config>(dto);
+ configBuffer.reinitAllBuffers(updateConfig);
+
+ ARMARX_INFO << "Trajectory with " << updateConfig.targets.trajectory.points().size();
+
+ ARMARX_IMPORTANT << "done Controller::updateConfig";
+ }
+
+ void
+ Controller::additionalTask()
+ {
+ ARMARX_CHECK(trajectoryFollowingController.has_value());
+
+ // if trajectory is empty, set velocity to 0
+ if (configBuffer.getUpToDateReadBuffer().targets.trajectory.points().empty())
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Trajectory is empty!";
+
+ getWriterControlStruct().platformVelocityTargets.x = 0;
+ getWriterControlStruct().platformVelocityTargets.y = 0;
+ getWriterControlStruct().platformVelocityTargets.yaw = 0;
+ writeControlStruct();
+ return;
+ }
+
+ // update controller
+ const armarx::navigation::traj_ctrl::TrajectoryControllerResult result =
+ trajectoryFollowingController->control(
+ configBuffer.getUpToDateReadBuffer().targets.trajectory);
+
+ // store result
+ getWriterControlStruct().platformVelocityTargets.x = result.twist.linear.x();
+ getWriterControlStruct().platformVelocityTargets.y = result.twist.linear.y();
+ getWriterControlStruct().platformVelocityTargets.yaw = result.twist.angular.z();
+
+ writeControlStruct();
+ }
+
+ void
+ Controller::onPublish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& debugDrawer,
+ const DebugObserverInterfacePrx& debugObservers)
+ {
+ StringVariantBaseMap datafields;
+
+ datafields["vx"] = new Variant(rtGetControlStruct().platformVelocityTargets.x);
+ datafields["vy"] = new Variant(rtGetControlStruct().platformVelocityTargets.y);
+ datafields["vyaw"] = new Variant(rtGetControlStruct().platformVelocityTargets.yaw);
+ datafields["trajectory_points"] = new Variant(configBuffer.getUpToDateReadBuffer().targets.trajectory.points().size());
+
+ debugObservers->setDebugChannel(
+ common::ControllerTypeNames.to_name(common::ControllerType::PlatformTrajectory),
+ datafields);
+ }
+
+ void
+ Controller::onInitNJointController()
+ {
+ runTask("PlatformTrajectoryControllerAdditionalTask",
+ [&]
+ {
+ CycleUtil c(10);
+ getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+ ARMARX_IMPORTANT << "Create a new thread alone PlatformTrajectory controller";
+ while (getState() == eManagedIceObjectStarted)
+ {
+ if (isControllerActive() and rtReady.load())
+ {
+ ARMARX_VERBOSE << "additional task";
+ additionalTask();
+ }
+ c.waitForCycleDuration();
+ }
+ });
+
+ ARMARX_INFO << "PlatformTrajectoryVelocityController::onInitNJointController";
+ }
+
+ void
+ Controller::rtPreActivateController()
+ {
+ rtReady.store(true);
+ }
+
+
+ Controller::~Controller() = default;
+
+} // namespace armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
new file mode 100644
index 0000000000000000000000000000000000000000..2b47dc9808b4a127f86919ae733ebd259acc25cb
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/PlatformTrajectoryController.h
@@ -0,0 +1,149 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+
+#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
+
+namespace armarx
+{
+ class ControlTargetHolonomicPlatformVelocity;
+ class SensorValueHolonomicPlatformWithAbsolutePosition;
+} // namespace armarx
+
+
+namespace armarx::navigation::platform_controller::platform_trajectory
+{
+ struct Targets
+ {
+ core::Trajectory trajectory;
+ };
+
+ struct Config
+ {
+ using Params = traj_ctrl::TrajectoryFollowingControllerParams;
+
+ Params params;
+ Targets targets;
+ };
+
+ struct Target
+ {
+ struct
+ {
+ double x = 0;
+ double y = 0;
+ double yaw = 0;
+ } platformVelocityTargets;
+
+ void
+ reset()
+ {
+ platformVelocityTargets.x = 0;
+ platformVelocityTargets.y = 0;
+ platformVelocityTargets.yaw = 0;
+ }
+ };
+
+ struct Devices
+ {
+ ControlTargetHolonomicPlatformVelocity* platformTarget;
+ };
+
+ using NameValueMap = std::map<std::string, float>;
+
+ class Controller :
+ virtual public NJointControllerWithTripleBuffer<Target>,
+ virtual public armarx::control::ConfigurableNJointControllerInterface
+ {
+ public:
+ using ConfigPtrT = control::ConfigurableNJointControllerConfigPtr;
+
+ Controller(const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
+
+ ~Controller() override;
+
+ std::string getClassName(const Ice::Current& iceCurrent = Ice::emptyCurrent) const override;
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
+ const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
+
+ protected:
+ void additionalTask();
+ void onPublish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& debugDrawer,
+ const DebugObserverInterfacePrx& debugObservers) override;
+
+
+ void onInitNJointController() override;
+ void rtPreActivateController() override;
+
+ private:
+ // [[nodiscard]] bool initializeQPIK();
+
+ // void updateDebugStatus();
+
+ // struct DebugStatus
+ // {
+ // std::map<std::string, float> data;
+ // };
+
+ TripleBuffer<Config> configBuffer;
+
+ // internal
+ std::atomic_bool rtFirstRun = true;
+ std::atomic_bool rtReady = false;
+
+ ControlTargetHolonomicPlatformVelocity* platformTarget;
+
+ std::optional<traj_ctrl::TrajectoryFollowingController> trajectoryFollowingController;
+
+
+ Devices getDevices(const VirtualRobot::RobotNodeSet& rns);
+
+ // NameValueMap getJointVelocities(const NameValueMap& initialJointValues,
+ // const NameValueMap& targetJointValues,
+ // const std::set<std::string>& joints);
+
+ // Eigen::VectorXd toOptikRobotState(const NameValueMap& jointValues) const;
+
+ // NameValueMap getJointValuesWithVirtualPlatformJoints();
+
+ // NameValueMap getPlatformJointVelocity(const NameValueMap& initialJointAngles,
+ // const NameValueMap& targetJointAngles) const;
+ };
+
+} // namespace armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml b/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a6551b537da4fc6a123fab11081dd346cdf74f29
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.xml
@@ -0,0 +1,43 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="../../core/aron/TwistLimits.xml" />
+ <Include include="../../core/aron/PIDParams.xml" />
+ <Include include="../../core/aron/Trajectory.xml" />
+ <Include include="../../trajectory_control/aron/TrajectoryFollowingControllerParams.xml" />
+ <!-- <Include include="<armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.xml>" /> -->
+ </AronIncludes>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<armarx/navigation/core/aron/TwistLimits.aron.generated.h>" />
+ <Include include="<armarx/navigation/core/aron/PIDParams.aron.generated.h>" />
+ <Include include="<armarx/navigation/core/aron/Trajectory.aron.generated.h>" />
+ <Include include="<armarx/navigation/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Params'>
+ <ObjectChild key='twistLimits'>
+ <armarx::navigation::core::arondto::TwistLimits />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Targets'>
+ <ObjectChild key='trajectory'>
+ <armarx::navigation::core::arondto::Trajectory />
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::navigation::platform_controller::platform_trajectory::arondto::Config'>
+ <ObjectChild key='params'>
+ <armarx::navigation::traj_ctrl::arondto::TrajectoryFollowingControllerParams />
+ </ObjectChild>
+
+ <ObjectChild key='targets'>
+ <armarx::navigation::platform_controller::platform_trajectory::arondto::Targets />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/platform_controller/aron_conversions.cpp b/source/armarx/navigation/platform_controller/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f32b69e9ea4195029c3fec019d75951a7a3ae78
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/aron_conversions.cpp
@@ -0,0 +1,50 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/eigen.h>
+
+#include <armarx/control/common/aron_conversions.h>
+#include <armarx/navigation/platform_controller/PlatformTrajectoryController.h>
+#include <armarx/navigation/core/aron_conversions.h>
+#include <armarx/navigation/trajectory_control/aron_conversions.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+
+
+
+namespace armarx::navigation::platform_controller::platform_trajectory
+{
+
+ void
+ fromAron(const arondto::Targets& dto, Targets& bo)
+ {
+ fromAron(dto.trajectory, bo.trajectory);
+ }
+
+ void
+ fromAron(const arondto::Config& dto, Config& bo)
+ {
+ fromAron(dto.params, bo.params);
+ fromAron(dto.targets, bo.targets);
+ }
+
+} // namespace armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/core/time/TimeServerInterface.h b/source/armarx/navigation/platform_controller/aron_conversions.h
similarity index 72%
rename from source/armarx/navigation/core/time/TimeServerInterface.h
rename to source/armarx/navigation/platform_controller/aron_conversions.h
index fdb74e854d19b41fd47a0db71a3e7a71f22fe0dd..bd5518b1b0f2e11f0cb3b2e0885f514073b5e92b 100644
--- a/source/armarx/navigation/core/time/TimeServerInterface.h
+++ b/source/armarx/navigation/platform_controller/aron_conversions.h
@@ -14,23 +14,22 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
+ * @date 2022
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
-#include "ArmarXCore/core/time/DateTime.h"
-namespace armarx::navigation::core
+namespace armarx::navigation::platform_controller::platform_trajectory
{
-
- class TimeServerInterface
+ namespace arondto
{
- public:
- virtual armarx::core::time::DateTime now() const = 0;
+ class Config;
+ }
+
+ struct Config;
- virtual ~TimeServerInterface() = default;
- };
-} // namespace armarx::navigation::core
+ void fromAron(const arondto::Config& dto, Config& bo);
+} // namespace armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/platform_controller/controller_descriptions.h b/source/armarx/navigation/platform_controller/controller_descriptions.h
new file mode 100644
index 0000000000000000000000000000000000000000..d352bed138bb228ce71ca0fd0ec3d1a28a936ea7
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/controller_descriptions.h
@@ -0,0 +1,44 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/control/client/ControllerDescription.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/platform_controller/PlatformTrajectoryController.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+
+namespace armarx::control::client
+{
+ template <>
+ struct ControllerDescription<armarx::navigation::common::ControllerType::PlatformTrajectory>
+ {
+ using AronDTO =
+ armarx::navigation::platform_controller::platform_trajectory::arondto::Config;
+
+ using BO = armarx::navigation::platform_controller::platform_trajectory::Config;
+
+ // constexpr static const char* name = armarx::navigation::common::ControllerTypeNames.to_name(armarx::navigation::common::ControllerType::PlatformTrajectory);
+ constexpr static const char* name =
+ armarx::navigation::common::PlatformTrajectoryControllerName;
+ };
+
+} // namespace armarx::control::client
diff --git a/source/armarx/navigation/platform_controller/json_conversions.cpp b/source/armarx/navigation/platform_controller/json_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7134777562b10432cd1dcab066daf3e3dd045f69
--- /dev/null
+++ b/source/armarx/navigation/platform_controller/json_conversions.cpp
@@ -0,0 +1,6 @@
+#include "json_conversions.h"
+
+namespace armarx_navigation::platform_controller
+{
+
+}
diff --git a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h b/source/armarx/navigation/platform_controller/json_conversions.h
similarity index 65%
rename from source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h
rename to source/armarx/navigation/platform_controller/json_conversions.h
index 31d6cf7e5f7c521aead76f179ef56b83f20f6f2a..6abd40df00ce418f7da923763f8fc2e8ab684893 100644
--- a/source/armarx/navigation/core/time/ChronoMonotonicTimeServer.h
+++ b/source/armarx/navigation/platform_controller/json_conversions.h
@@ -14,25 +14,17 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
+ * @date 2022
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
-#include <armarx/navigation/core/time/TimeServerInterface.h>
-#include <armarx/navigation/core/types.h>
+#include <SimoxUtility/json.h>
-namespace armarx::navigation::core
+namespace armarx::navigation::platform_controller::platform_trajectory
{
- class ChronoMonotonicTimeServer : virtual public TimeServerInterface
- {
- public:
- armarx::core::time::DateTime now() const override;
-
- ~ChronoMonotonicTimeServer() override = default;
- };
-} // namespace armarx::navigation::core
+} // namespace armarx::navigation::platform_controller::platform_trajectory
diff --git a/source/armarx/navigation/server/CMakeLists.txt b/source/armarx/navigation/server/CMakeLists.txt
index 25eae2276eda76c413929edad42321ff7d108948..a7dad944f4c1d0540fc3d2333664d52ef8e06862 100644
--- a/source/armarx/navigation/server/CMakeLists.txt
+++ b/source/armarx/navigation/server/CMakeLists.txt
@@ -1,52 +1,65 @@
armarx_add_library(server
SOURCES
- ./Navigator.cpp
- ./StackResult.cpp
- ./GraphBuilder.cpp
+ Navigator.cpp
+ StackResult.cpp
+ GraphBuilder.cpp
# Executors.
- ./execution/PlatformUnitExecutor.cpp
+ # ./execution/PlatformUnitExecutor.cpp
+ execution/PlatformControllerExecutor.cpp
# Event publishing.
- ./event_publishing/MemoryPublisher.cpp
+ event_publishing/MemoryPublisher.cpp
# Introspection
- ./introspection/ArvizIntrospector.cpp
- ./introspection/MemoryIntrospector.cpp
+ introspection/ArvizIntrospector.cpp
+ introspection/MemoryIntrospector.cpp
# monitoring
- ./monitoring/GoalReachedMonitor.cpp
+ monitoring/GoalReachedMonitor.cpp
# parameterization
- ./parameterization/MemoryParameterizationService.cpp
+ parameterization/MemoryParameterizationService.cpp
+ # scene provider
+ scene_provider/SceneProvider.cpp
HEADERS
- ./Navigator.h
- ./NavigationStack.h
- ./StackResult.h
- ./GraphBuilder.h
+ Navigator.h
+ NavigationStack.h
+ StackResult.h
+ GraphBuilder.h
# Executors.
- ./execution/ExecutorInterface.h
- ./execution/PlatformUnitExecutor.h
- ./execution/DummyExecutor.h
+ execution/ExecutorInterface.h
+ # ./execution/PlatformUnitExecutor.h
+ execution/PlatformControllerExecutor.h
+ execution/DummyExecutor.h
# Event publishing.
- ./event_publishing/EventPublishingInterface.h
- ./event_publishing/MemoryPublisher.h
+ event_publishing/EventPublishingInterface.h
+ event_publishing/MemoryPublisher.h
# Introspection
- ./introspection/IntrospectorInterface.h
- ./introspection/ArvizIntrospector.h
- ./introspection/MemoryIntrospector.h
+ introspection/IntrospectorInterface.h
+ introspection/ArvizIntrospector.h
+ introspection/MemoryIntrospector.h
# monitoring
- ./monitoring/GoalReachedMonitor.h
+ monitoring/GoalReachedMonitor.h
# parameterization
- ./parameterization/MemoryParameterizationService.h
+ parameterization/MemoryParameterizationService.h
+ # scene provider
+ scene_provider/SceneProviderInterface.h
+ scene_provider/SceneProvider.h
DEPENDENCIES_PUBLIC
- ArmarXCoreInterfaces
- ArmarXCore
- ArViz # RobotAPI
- armarx_navigation::core
- armarx_navigation::util
- armarx_navigation::global_planning
- armarx_navigation::local_planning
- armarx_navigation::trajectory_control
- armarx_navigation::safety_control
- armarx_navigation::memory
+ ArmarXCoreInterfaces
+ ArmarXCore
+ # RobotAPI
+ ArViz
+ armem_robot
+ armem_robot_state
+ # armarx_navigation
+ armarx_navigation::core
+ armarx_navigation::util
+ armarx_navigation::global_planning
+ armarx_navigation::local_planning
+ armarx_navigation::trajectory_control
+ armarx_navigation::safety_control
+ armarx_navigation::memory
+ armarx_control::client
+ armarx_control::interface
DEPENDENCIES_PRIVATE
- range-v3::range-v3
+ range-v3::range-v3
)
armarx_add_test(server_test
diff --git a/source/armarx/navigation/server/NavigationStack.h b/source/armarx/navigation/server/NavigationStack.h
index 9c047123f4a851cb85448f5ba3424ac3a4e625be..c2a76f9a2269400f62d3e699d503036159db7a47 100644
--- a/source/armarx/navigation/server/NavigationStack.h
+++ b/source/armarx/navigation/server/NavigationStack.h
@@ -34,7 +34,7 @@ namespace armarx::navigation::server
{
global_planning::GlobalPlannerPtr globalPlanner;
loc_plan::LocalPlannerPtr localPlanner = nullptr;
- traj_ctrl::TrajectoryControllerPtr trajectoryControl;
- safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
+ // traj_ctrl::TrajectoryControllerPtr trajectoryControl;
+ // safe_ctrl::SafetyControllerPtr safetyControl = nullptr;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 3a1b42f2c58f29c5eb7164cbab8921c86055762e..e43dfa69a63dd9af6a0fd1a30d4740a2463b85e8 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -18,11 +18,14 @@
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <VirtualRobot/Robot.h>
+#include "ArmarXCore/core/time/StopWatch.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include "armarx/navigation/local_planning/LocalPlanner.h"
#include "range/v3/algorithm/reverse.hpp"
#include "range/v3/range/conversion.hpp"
#include <SemanticObjectRelations/Shapes/Shape.h>
@@ -33,12 +36,13 @@
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/events.h>
-#include <armarx/navigation/core/time/TimeServerInterface.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/server/GraphBuilder.h>
#include <armarx/navigation/server/StackResult.h>
#include <armarx/navigation/server/monitoring/GoalReachedMonitor.h>
+#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
+
namespace armarx::navigation::server
{
@@ -54,7 +58,7 @@ namespace armarx::navigation::server
Navigator::Navigator(const Config& config, const InjectedServices& services) :
config{config}, srv{services}
{
- ARMARX_CHECK_NOT_NULL(config.scene) << "The scene must be set!";
+ ARMARX_CHECK_NOT_NULL(srv.sceneProvider) << "The scene provider must be set!";
ARMARX_CHECK_NOT_NULL(services.executor) << "The executor service must be set!";
ARMARX_CHECK_NOT_NULL(services.publisher) << "The publisher service must be set!";
}
@@ -147,11 +151,12 @@ namespace armarx::navigation::server
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(config.scene->robot->getGlobalPose()) * p; });
+ 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;
}
@@ -381,7 +386,7 @@ namespace armarx::navigation::server
.velocity = 0};
// resample event straight lines
-// ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
+ // ARMARX_CHECK_NOT_EMPTY(trajectoryPoints);
// const core::Trajectory segmentTraj({trajectoryPoints.back(), nextTrajPt});
// ARMARX_INFO << "Segment trajectory length: " << segmentTraj.length();
@@ -428,7 +433,7 @@ namespace armarx::navigation::server
{
//
GraphBuilder graphBuilder;
- graphBuilder.initialize(core::Pose(config.scene->robot->getGlobalPose()));
+ graphBuilder.initialize(core::Pose(srv.sceneProvider->scene().robot->getGlobalPose()));
// std::optional<Graph*> activeSubgraph;
for (const auto& target : targets)
@@ -451,7 +456,7 @@ namespace armarx::navigation::server
// if (not target.locationId->empty())
// {
- // const auto& subgraph = core::getSubgraph(target.locationId.value(),config.scene->graph->subgraphs);
+ // const auto& subgraph = core::getSubgraph(target.locationId.value(),srv.sceneProvider->scene().graph->subgraphs);
// return subgraph.name() != activeSubgraph;
// }
@@ -479,8 +484,8 @@ namespace armarx::navigation::server
// instead. Thus, we collect all routes to reach the node.
if (not target.locationId->empty())
{
- const auto& subgraph =
- core::getSubgraph(target.locationId.value(), config.scene->graph->subgraphs);
+ const auto& subgraph = core::getSubgraph(
+ target.locationId.value(), srv.sceneProvider->scene().graph->subgraphs);
const auto vertex = core::getVertexByName(target.locationId.value(), subgraph);
@@ -542,8 +547,8 @@ namespace armarx::navigation::server
ARMARX_TRACE;
goalReachedMonitor = std::nullopt;
- goalReachedMonitor =
- GoalReachedMonitor(graphBuilder.goalPose(), *config.scene, GoalReachedMonitorConfig());
+ goalReachedMonitor = GoalReachedMonitor(
+ graphBuilder.goalPose(), srv.sceneProvider->scene(), GoalReachedMonitorConfig());
if (goalReachedMonitor->goalReached())
{
@@ -551,9 +556,9 @@ namespace armarx::navigation::server
<< goalReachedMonitor->goal().translation().head<2>()
<< ". Robot won't move.";
- srv.publisher->goalReached(
- core::GoalReachedEvent{{config.scene->timeServer->now()},
- core::Pose(config.scene->robot->getGlobalPose())});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
return;
}
@@ -572,7 +577,7 @@ namespace armarx::navigation::server
ARMARX_TRACE;
std::vector<core::Pose> vertexPoses;
- vertexPoses.emplace_back(config.scene->robot->getGlobalPose());
+ vertexPoses.emplace_back(srv.sceneProvider->scene().robot->getGlobalPose());
ARMARX_INFO << "Navigating along the following nodes:";
for (const semrel::ShapeID& vertex : shortestPath)
@@ -600,12 +605,12 @@ namespace armarx::navigation::server
// the following is similar to moveToAbsolute
// TODO(fabian.reister): remove code duplication
- ARMARX_TRACE;
+ srv.executor->execute(globalPlan->trajectory);
+ ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
ARMARX_TRACE;
-
srv.introspector->onGlobalPlannerResult(globalPlan.value());
ARMARX_INFO << "Global planning completed. Will now start all required threads";
@@ -618,79 +623,21 @@ namespace armarx::navigation::server
Navigator::startStack()
{
- // local planner
- if (config.stack.localPlanner)
- {
- ARMARX_TRACE;
-
- ARMARX_INFO << "Planning local trajectory enabled";
- localPlannerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateLocalPlanner,
- config.general.tasks.localPlanningUpdatePeriod,
- false,
- "LocalPlannerTask");
- localPlannerTask->start();
- }
-
- // trajectory controller
- ARMARX_CHECK_NOT_NULL(config.stack.trajectoryControl);
- trajectoryControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateTrajectoryController,
- config.general.tasks.trajectoryControllerUpdatePeriod,
- false,
- "TrajectoryControllerTask");
- trajectoryControllerTask->start();
-
- // safety controller
- if (config.stack.safetyControl)
- {
- ARMARX_INFO << "Safety control enabled";
- safetyControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateSafetyController,
- config.general.tasks.safetyControllerUpdatePeriod,
- false,
- "SafetyControllerTask");
- safetyControllerTask->start();
- }
+ ARMARX_TRACE;
- // executor
- executorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateExecutor,
- config.general.tasks.executorUpdatePeriod,
- false,
- "ExecutorTask");
- executorTask->start();
-
- // introspector
- introspectorTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateIntrospector,
- config.general.tasks.introspectorUpdatePeriod,
- false,
- "IntrospectorTask");
- introspectorTask->start();
-
- // monitoring
- ARMARX_INFO << "Starting monitoring";
- // std::vector<core::Pose> waypoints = globalPlan->trajectory.poses();
-
-
- // goalReachedMonitor =
- // GoalReachedMonitor(waypoints.back(), *config.scene, GoalReachedMonitorConfig());
- monitorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateMonitor,
- config.general.tasks.monitorUpdatePeriod,
+ ARMARX_INFO << "Planning local trajectory enabled";
+ runningTask = new PeriodicTask<Navigator>(this,
+ &Navigator::run,
+ config.general.tasks.replanningUpdatePeriod,
false,
- "MonitorTask");
- monitorTask->start();
+ "LocalPlannerTask");
+ runningTask->start();
+
// Could be required if pauseMovement() has been called in the past.
resume();
srv.publisher->movementStarted(core::MovementStartedEvent{
- {config.scene->timeServer->now()},core::Pose(config.scene->robot->getGlobalPose())});
+ {armarx::Clock::Now()}, core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
}
void
@@ -708,21 +655,23 @@ namespace armarx::navigation::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_EMPTY(waypoints);
- ARMARX_INFO << "Request to move from " << config.scene->robot->getGlobalPose() << " to "
- << waypoints.back().matrix();
+ srv.sceneProvider->synchronize(Clock::Now());
+
+ ARMARX_INFO << "Request to move from " << srv.sceneProvider->scene().robot->getGlobalPose()
+ << " to " << waypoints.back().matrix();
// first we check if we are already at the goal position
goalReachedMonitor = std::nullopt;
- goalReachedMonitor =
- GoalReachedMonitor(waypoints.back(), *config.scene, GoalReachedMonitorConfig());
+ 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{{config.scene->timeServer->now()},
- core::Pose(config.scene->robot->getGlobalPose())});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())});
return;
}
@@ -741,92 +690,22 @@ namespace armarx::navigation::server
if (not globalPlan.has_value())
{
ARMARX_WARNING << "No global trajectory. Cannot move.";
- srv.publisher->globalPlanningFailed(core::GlobalPlanningFailedEvent{
- {.timestamp = config.scene->timeServer->now()}, {""}});
+ srv.publisher->globalPlanningFailed(
+ core::GlobalPlanningFailedEvent{{.timestamp = armarx::Clock::Now()}, {""}});
srv.introspector->failure();
return;
}
- ARMARX_TRACE;
+ ARMARX_TRACE;
srv.publisher->globalTrajectoryUpdated(globalPlan.value());
- // srv.introspector->onGlobalPlan(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;
- // local planner
- if (config.stack.localPlanner)
- {
- ARMARX_INFO << "Planning local trajectory enabled";
- localPlannerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateLocalPlanner,
- config.general.tasks.localPlanningUpdatePeriod,
- false,
- "LocalPlannerTask");
- localPlannerTask->start();
- }
-
- // trajectory controller
- ARMARX_CHECK_NOT_NULL(config.stack.trajectoryControl);
- trajectoryControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateTrajectoryController,
- config.general.tasks.trajectoryControllerUpdatePeriod,
- false,
- "TrajectoryControllerTask");
- trajectoryControllerTask->start();
-
- // safety controller
- if (config.stack.safetyControl)
- {
- ARMARX_INFO << "Safety control enabled";
- safetyControllerTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateSafetyController,
- config.general.tasks.safetyControllerUpdatePeriod,
- false,
- "SafetyControllerTask");
- safetyControllerTask->start();
- }
-
- // executor
- executorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateExecutor,
- config.general.tasks.executorUpdatePeriod,
- false,
- "ExecutorTask");
- executorTask->start();
-
- // introspector
- introspectorTask =
- new PeriodicTask<Navigator>(this,
- &Navigator::updateIntrospector,
- config.general.tasks.introspectorUpdatePeriod,
- false,
- "IntrospectorTask");
- introspectorTask->start();
-
- // monitoring
- ARMARX_INFO << "Starting monitoring";
-
- monitorTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateMonitor,
- config.general.tasks.monitorUpdatePeriod,
- false,
- "MonitorTask");
- monitorTask->start();
-
- // Could be required if pauseMovement() has been called in the past.
- ARMARX_INFO << "Resuming ...";
- ARMARX_TRACE;
- resume();
- ARMARX_INFO << "Done.";
-
- // TODO check if startPose shouldn't be waypoints.front() etc
- srv.publisher->movementStarted(core::MovementStartedEvent{
- {config.scene->timeServer->now()}, {waypoints.back()}});
+ startStack();
ARMARX_INFO << "Movement started.";
}
@@ -841,48 +720,81 @@ namespace armarx::navigation::server
{
}
+
void
- Navigator::updateLocalPlanner()
+ Navigator::run()
{
- localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
+ // TODO(fabian.reister): add debug observer logging
- if (localPlan.has_value())
+ // scene update
{
- srv.publisher->localTrajectoryUpdated(localPlan.value());
+ ARMARX_DEBUG << "Updating scene";
+
+ const Duration duration =
+ armarx::core::time::StopWatch::measure([&]() { updateScene(); });
+
+ ARMARX_VERBOSE << deactivateSpam(0.2) << "Scene update: " << duration.toMilliSecondsDouble()
+ << "ms.";
}
- else
+
+ // local planner update
{
- // srv.publisher->localTrajectoryPlanningFailed();
- }
- }
+ ARMARX_DEBUG << "Local planner update";
- void
- Navigator::updateTrajectoryController()
- {
- // TODO distinguish between global and local plan!
+ const Duration duration = armarx::core::time::StopWatch::measure(
+ [&]()
+ {
+ if (hasLocalPlanner())
+ {
+ const auto localPlannerResult = updateLocalPlanner();
+ updateExecutor(localPlannerResult);
+ updateIntrospector(localPlannerResult);
+ }
+ });
+ ARMARX_VERBOSE << deactivateSpam(0.2)
+ << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
+ }
- if (not globalPlan.has_value())
+ // monitor update
{
- return;
- }
+ ARMARX_DEBUG << "Monitor update";
- trajectoryCtrlResult = config.stack.trajectoryControl->control(globalPlan->trajectory);
+ const Duration duration =
+ armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
- srv.publisher->trajectoryControllerUpdated(trajectoryCtrlResult.value());
+ ARMARX_VERBOSE << deactivateSpam(0.2)
+ << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
+ }
}
void
- Navigator::updateSafetyController()
+ Navigator::updateScene()
+ {
+ ARMARX_CHECK_NOT_NULL(srv.sceneProvider);
+ srv.sceneProvider->synchronize(armarx::Clock::Now());
+ }
+
+ std::optional<loc_plan::LocalPlannerResult>
+ Navigator::updateLocalPlanner()
{
- // FIXME
- // if (res.controlVelocity)
- // {
- // res.safeVelocity = config.stack.safetyControl->control(res.controlVelocity.value());
- // }
+ ARMARX_CHECK(hasLocalPlanner());
+
+ localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
+
+ if (localPlan.has_value())
+ {
+ srv.publisher->localTrajectoryUpdated(localPlan.value());
+ }
+ else
+ {
+ // srv.publisher->localTrajectoryPlanningFailed();
+ }
+
+ return localPlan;
}
void
- Navigator::updateExecutor()
+ Navigator::updateExecutor(const std::optional<loc_plan::LocalPlannerResult>& localPlan)
{
if (isPaused() or isStopped())
{
@@ -891,71 +803,42 @@ namespace armarx::navigation::server
return;
}
- if (not isStackResultValid())
+ if (not localPlan.has_value())
{
- ARMARX_VERBOSE << deactivateSpam(1) << "stack result invalid";
- // [[unlikely]]
- return;
+ ARMARX_INFO << "Local plan is invalid!";
+ srv.executor->stop();
}
- const core::Twist twist = [&]() -> core::Twist
- {
- if (config.stack.safetyControl != nullptr)
- {
- return safetyCtrlResult->twist;
- }
- return trajectoryCtrlResult->twist;
- }();
+ // const core::Rotation robot_R_world(srv.sceneProvider->scene().robot->getGlobalOrientation().inverse());
- const core::Rotation robot_R_world(config.scene->robot->getGlobalOrientation().inverse());
+ // ARMARX_VERBOSE
+ // << deactivateSpam(100) << "Robot orientation "
+ // << simox::math::mat3f_to_rpy(srv.sceneProvider->scene().robot->getGlobalOrientation()).z();
- ARMARX_VERBOSE
- << deactivateSpam(100) << "Robot orientation "
- << simox::math::mat3f_to_rpy(config.scene->robot->getGlobalOrientation()).z();
+ // core::Twist robotFrameVelocity;
- core::Twist robotFrameVelocity;
+ // robotFrameVelocity.linear = robot_R_world * twist.linear;
+ // // FIXME fix angular velocity
+ // robotFrameVelocity.angular = twist.angular;
- robotFrameVelocity.linear = robot_R_world * twist.linear;
- // FIXME fix angular velocity
- robotFrameVelocity.angular = twist.angular;
+ // ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
+ // << robotFrameVelocity.linear;
- ARMARX_VERBOSE << deactivateSpam(1) << "velocity in robot frame "
- << robotFrameVelocity.linear;
-
- srv.executor->move(robotFrameVelocity);
+ srv.executor->execute(localPlan->trajectory);
}
void
- Navigator::updateIntrospector()
+ Navigator::updateIntrospector(const std::optional<loc_plan::LocalPlannerResult>& localPlan)
{
ARMARX_CHECK_NOT_NULL(srv.introspector);
- // TODO event driven. should be committed only on change
- if(globalPlan.has_value())
- {
- srv.introspector->onGlobalPlannerResult(globalPlan.value());
- }
-
- if (localPlan.has_value())
- {
- srv.introspector->onLocalPlannerResult(localPlan.value());
- }
-
- if (trajectoryCtrlResult.has_value())
- {
- srv.introspector->onTrajectoryControllerResult(trajectoryCtrlResult.value());
- }
-
- if (safetyCtrlResult.has_value())
+ if (not localPlan.has_value())
{
- srv.introspector->onSafetyControllerResult(safetyCtrlResult.value());
+ return;
}
- // if (res.isValid())
- // {
- // srv.introspector->onStackResult(res);
- // }
+ srv.introspector->onLocalPlannerResult(localPlan.value());
}
void
@@ -973,9 +856,9 @@ namespace armarx::navigation::server
stop();
- srv.publisher->goalReached(
- core::GoalReachedEvent{{config.scene->timeServer->now()},
- {core::Pose(config.scene->robot->getGlobalPose())}});
+ srv.publisher->goalReached(core::GoalReachedEvent{
+ {armarx::Clock::Now()},
+ {core::Pose(srv.sceneProvider->scene().robot->getGlobalPose())}});
srv.introspector->success();
@@ -991,61 +874,48 @@ namespace armarx::navigation::server
void
Navigator::stopAllThreads()
{
- stopIfRunning(localPlannerTask);
- stopIfRunning(trajectoryControllerTask);
- stopIfRunning(safetyControllerTask);
- stopIfRunning(executorTask);
- stopIfRunning(introspectorTask);
- stopIfRunning(monitorTask);
+ stopIfRunning(runningTask);
}
- bool
- Navigator::isStackResultValid() const noexcept
- {
-
- // global planner
- if (config.stack.globalPlanner != nullptr)
- {
- if (not globalPlan.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Global trajectory not yet set.";
- return false;
- }
- }
-
- // local planner
- if (config.stack.localPlanner != nullptr)
- {
- if (not localPlan.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Local trajectory not yet set.";
- return false;
- }
- }
-
- // trajectory controller
- if (config.stack.trajectoryControl != nullptr)
- {
- if (not trajectoryCtrlResult.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Raw control velocity not yet set.";
- return false;
- }
- }
-
- // safety controller
- if (config.stack.safetyControl != nullptr)
- {
- if (not safetyCtrlResult.has_value())
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Safe velocity not yet set.";
- return false;
- }
- }
-
- // [[likely]]
- return true;
- }
+ // bool
+ // Navigator::isStackResultValid() const noexcept
+ // {
+
+ // // global planner
+ // if (config.stack.globalPlanner != nullptr)
+ // {
+ // if (not globalPlan.has_value())
+ // {
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Global trajectory not yet set.";
+ // return false;
+ // }
+ // }
+
+ // // local planner
+ // if (config.stack.localPlanner != nullptr)
+ // {
+ // if (not localPlan.has_value())
+ // {
+ // ARMARX_VERBOSE << deactivateSpam(1) << "Local trajectory not yet set.";
+ // return false;
+ // }
+ // }
+
+ // // [[likely]]
+ // return true;
+ // }
+
+ // const core::Trajectory& Navigator::currentTrajectory() const
+ // {
+ // ARMARX_CHECK(isStackResultValid());
+
+ // if(localPlan.has_value())
+ // {
+ // return localPlan->trajectory;
+ // }
+
+ // return globalPlan->trajectory;
+ // }
void
Navigator::pause()
@@ -1055,7 +925,7 @@ namespace armarx::navigation::server
executorEnabled.store(false);
ARMARX_CHECK_NOT_NULL(srv.executor);
- srv.executor->move(core::Twist::Zero());
+ srv.executor->stop();
}
void
@@ -1064,6 +934,8 @@ namespace armarx::navigation::server
ARMARX_INFO << "Resume.";
executorEnabled.store(true);
+
+ srv.executor->start();
}
void
diff --git a/source/armarx/navigation/server/Navigator.h b/source/armarx/navigation/server/Navigator.h
index a1ccfa3e3cc9cfd4ce9d4459fe7699931d48bd8f..a0affd5d1fad6ebbe6787e3132d65939e823266d 100644
--- a/source/armarx/navigation/server/Navigator.h
+++ b/source/armarx/navigation/server/Navigator.h
@@ -52,6 +52,11 @@
namespace armarx::navigation::server
{
+ namespace scene_provider
+ {
+ class SceneProviderInterface;
+ }
+
class Navigator : virtual public core::NavigatorInterface
{
@@ -62,18 +67,11 @@ namespace armarx::navigation::server
{
struct
{
- // TODO: Use chrono?
- int localPlanningUpdatePeriod{100}; // [ms]
- int trajectoryControllerUpdatePeriod{100}; // [ms]
- int safetyControllerUpdatePeriod{100}; // [ms]
- int introspectorUpdatePeriod{100}; // [ms]
- int executorUpdatePeriod{100}; // [ms]
- int monitorUpdatePeriod{20}; // [ms]
+ int replanningUpdatePeriod{100}; // [ms]
} tasks;
};
server::NavigationStack stack;
- core::Scene* const scene;
General general;
};
@@ -82,9 +80,10 @@ namespace armarx::navigation::server
ExecutorInterface* executor;
EventPublishingInterface* publisher;
IntrospectorInterface* introspector = nullptr;
+
+ scene_provider::SceneProviderInterface* sceneProvider;
};
- public:
Navigator(const Config& config, const InjectedServices& services);
void moveTo(const std::vector<core::Pose>& waypoints,
@@ -117,16 +116,18 @@ namespace armarx::navigation::server
void moveToAbsolute(const std::vector<core::Pose>& waypoints);
void moveTowardsAbsolute(const core::Direction& direction);
- void updateLocalPlanner();
- void updateTrajectoryController();
- void updateSafetyController();
+ void run();
+
+ void updateScene();
- void updateExecutor();
+ std::optional<loc_plan::LocalPlannerResult> updateLocalPlanner();
+ void updateExecutor(const std::optional<loc_plan::LocalPlannerResult>& localPlan);
+ void updateIntrospector(const std::optional<loc_plan::LocalPlannerResult>& localPlan);
- void updateIntrospector();
void updateMonitor();
- bool isStackResultValid() const noexcept;
+ // const core::Trajectory& currentTrajectory() const;
+ // bool isStackResultValid() const noexcept;
void stopAllThreads();
@@ -136,6 +137,10 @@ namespace armarx::navigation::server
void setGraphEdgeCosts(core::Graph& graph) const;
GraphBuilder convertToGraph(const std::vector<client::WaypointTarget>& targets) const;
+ bool hasLocalPlanner() const noexcept
+ {
+ return config.stack.localPlanner != nullptr;
+ }
Config config;
@@ -143,25 +148,13 @@ namespace armarx::navigation::server
std::atomic_bool executorEnabled = true;
- // StackResult res;
-
- PeriodicTask<Navigator>::pointer_type localPlannerTask;
- PeriodicTask<Navigator>::pointer_type trajectoryControllerTask;
- PeriodicTask<Navigator>::pointer_type safetyControllerTask;
-
- PeriodicTask<Navigator>::pointer_type executorTask;
-
- PeriodicTask<Navigator>::pointer_type introspectorTask;
-
- PeriodicTask<Navigator>::pointer_type monitorTask;
+ PeriodicTask<Navigator>::pointer_type runningTask;
std::optional<GoalReachedMonitor> goalReachedMonitor;
std::optional<global_planning::GlobalPlannerResult> globalPlan;
std::optional<loc_plan::LocalPlannerResult> localPlan;
- std::optional<traj_ctrl::TrajectoryControllerResult> trajectoryCtrlResult;
- std::optional<safe_ctrl::SafetyControllerResult> safetyCtrlResult;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/ExecutorInterface.h b/source/armarx/navigation/server/execution/ExecutorInterface.h
index e0ba451ebd0080e4a0f951e8e414e16beb5dc98e..ea77fd7a6d2c41bd5ec7bc375a37cba1c2d1ec57 100644
--- a/source/armarx/navigation/server/execution/ExecutorInterface.h
+++ b/source/armarx/navigation/server/execution/ExecutorInterface.h
@@ -2,7 +2,7 @@
namespace armarx::navigation::core
{
- struct Twist;
+ class Trajectory;
} // namespace armarx::navigation::core
namespace armarx::navigation::server
@@ -17,7 +17,10 @@ namespace armarx::navigation::server
public:
virtual ~ExecutorInterface() = default;
- virtual void move(const core::Twist& twist) = 0;
+ virtual void execute(const core::Trajectory& trajectory) = 0;
+
+ virtual void start() = 0;
+ virtual void stop() = 0;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f089038f82c2b6b3e70a4ea20755fe824a45654
--- /dev/null
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp
@@ -0,0 +1,96 @@
+#include "PlatformControllerExecutor.h"
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <armarx/control/client/ComponentPlugin.h>
+#include <armarx/control/common/type.h>
+#include <armarx/control/memory/config/util.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/core/aron_conversions.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/aron_conversions.h>
+#include <armarx/navigation/platform_controller/controller_descriptions.h>
+#include <armarx/navigation/platform_controller/json_conversions.h>
+
+
+namespace armarx::navigation::server
+{
+
+ PlatformControllerExecutor::PlatformControllerExecutor(
+ ControllerComponentPlugin& controllerComponentPlugin) :
+ controllerPlugin(controllerComponentPlugin)
+ {
+ ARMARX_TRACE;
+ controllerComponentPlugin.getRobotUnitPlugin().getRobotUnit()->loadLibFromPackage(
+ "armarx_navigation", "libarmarx_navigation_platform_controller.so");
+
+ // make default configs available to the memory
+ ARMARX_INFO << "Loading default configs";
+ {
+ // const std::filesystem::path configBasePath =
+ // PackagePath("armarx_navigation", "controller_config").toSystemPath();
+
+ ARMARX_TRACE;
+ // armarx::control::memory::config::parseAndStoreDefaultConfigs<
+ // armarx::navigation::common::ControllerType::PlatformTrajectory>(
+ // "" /*configBasePath*/, controllerComponentPlugin.configMemoryWriter());
+
+ ARMARX_TRACE;
+ ARMARX_INFO << "asdlfasfdlh";
+ }
+
+ // initialize controller
+ ARMARX_INFO << "Initializing controller";
+ {
+ ARMARX_TRACE;
+ auto builder = controllerPlugin.createControllerBuilder<
+ armarx::navigation::common::ControllerType::PlatformTrajectory>();
+
+ ARMARX_TRACE;
+
+ const armarx::PackagePath configPath("armarx_navigation", "controller_config/PlatformTrajectory/default.json");
+
+ auto ctrlWrapper =
+ builder.withNodeSet("PlatformPlanning")
+ .withConfig(configPath.toSystemPath())
+ .create();
+
+ ARMARX_TRACE;
+ ARMARX_CHECK(ctrlWrapper.has_value());
+ ctrl.emplace(std::move(ctrlWrapper.value()));
+ }
+
+ ARMARX_TRACE;
+ ARMARX_CHECK(ctrl.has_value());
+ ARMARX_INFO << "PlatformControllerExecutor: init done.";
+ }
+
+ PlatformControllerExecutor::~PlatformControllerExecutor() = default;
+
+
+ void
+ PlatformControllerExecutor::execute(const core::Trajectory& trajectory)
+ {
+ ARMARX_INFO << "Received trajectory for execution with " << trajectory.points().size()
+ << " points";
+
+ toAron(ctrl->config.targets.trajectory, trajectory);
+
+ // sends the updated config to the controller and stores it in the memory
+ ctrl->updateConfig();
+ }
+
+ void
+ PlatformControllerExecutor::start()
+ {
+ // TODO check whether the controller must be resetted (trajectory)
+ ctrl->activate();
+ }
+
+ void
+ PlatformControllerExecutor::stop()
+ {
+ ctrl->deactivate();
+ }
+} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/execution/PlatformControllerExecutor.h b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
new file mode 100644
index 0000000000000000000000000000000000000000..d256904cefa0cee344faeea3bddc4b9197f70c60
--- /dev/null
+++ b/source/armarx/navigation/server/execution/PlatformControllerExecutor.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include <armarx/control/client/ComponentPlugin.h>
+#include <armarx/navigation/common/controller_types.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
+#include <armarx/navigation/platform_controller/controller_descriptions.h>
+#include <armarx/navigation/server/execution/ExecutorInterface.h>
+
+namespace armarx::control::client
+{
+ class ComponentPlugin;
+}
+
+namespace armarx::navigation::server
+{
+
+ /**
+ * @brief The PlatformUnitExecutor class
+ *
+ * TODO: Should be renamed to whatever the new unit will be called which takes Mat4f
+ * as input instead of X/Y/Yaw, so that we have a generic interface.
+ */
+ class PlatformControllerExecutor : virtual public ExecutorInterface
+ {
+
+ public:
+ using ControllerComponentPlugin = armarx::control::client::ComponentPlugin;
+
+ PlatformControllerExecutor(ControllerComponentPlugin& controllerComponentPlugin);
+ ~PlatformControllerExecutor() override;
+
+ void execute(const core::Trajectory& trajectory) override;
+
+ void start() override;
+ void stop() override;
+
+ private:
+ std::optional<armarx::control::client::ControllerWrapper<
+ armarx::navigation::common::ControllerType::PlatformTrajectory>>
+ ctrl;
+
+ ControllerComponentPlugin& controllerPlugin;
+ };
+
+} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index b6c88e55bf8def186f58c848586770d2adeb3854..0f107c9907a91ef08c2ef99ba3b4c4c0b2330491 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -11,7 +11,7 @@
#include <SimoxUtility/color/ColorMap.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <VirtualRobot/Robot.h>
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
@@ -59,22 +59,22 @@ namespace armarx::navigation::server
drawLocalTrajectory(result.trajectory);
}
- void
- ArvizIntrospector::onTrajectoryControllerResult(
- const traj_ctrl::TrajectoryControllerResult& result)
- {
- std::lock_guard g{mtx};
+ // void
+ // ArvizIntrospector::onTrajectoryControllerResult(
+ // const traj_ctrl::TrajectoryControllerResult& result)
+ // {
+ // std::lock_guard g{mtx};
- drawRawVelocity(result.twist);
- }
+ // drawRawVelocity(result.twist);
+ // }
- void
- ArvizIntrospector::onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result)
- {
- std::lock_guard g{mtx};
+ // void
+ // ArvizIntrospector::onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result)
+ // {
+ // std::lock_guard g{mtx};
- drawSafeVelocity(result.twist);
- }
+ // drawSafeVelocity(result.twist);
+ // }
void
ArvizIntrospector::onGoal(const core::Pose& goal)
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index e91d4b608df6ed1517bb8b8cd864f195267b8bd6..a3daddc48d80315c081f1d3169bf251824d1b69c 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -54,10 +54,7 @@ namespace armarx::navigation::server
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) override;
- void
- onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) override;
- void onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result) override;
-
+
void onGoal(const core::Pose& goal) override;
void success();
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 92176601e325c9d2912c2f88e061afaea83abc83..18eaa36bc1368345ae3d73add730566c17883203 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -44,9 +44,6 @@ namespace armarx::navigation::server
virtual void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) = 0;
virtual void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) = 0;
- virtual void
- onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) = 0;
- virtual void onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result) = 0;
virtual void onGlobalShortestPath(const std::vector<core::Pose>& path) = 0;
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
index 226a0c80806a9700efcff8231f00b418a62c6d83..aaf84a09d75d103ba358908b44326d25a300f6ac 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
@@ -22,18 +22,6 @@ namespace armarx::navigation::server
// TODO(fabian.reister): implement
}
- void
- MemoryIntrospector::onTrajectoryControllerResult(
- const traj_ctrl::TrajectoryControllerResult& result)
- {
- // TODO(fabian.reister): implement
- }
-
- void
- MemoryIntrospector::onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result)
- {
- // TODO(fabian.reister): implement
- }
void
MemoryIntrospector::onGoal(const core::Pose& goal)
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index a141d4ba648d13c08e1ec8dacd9dc35c2944dbcf..9d473a7457777cec2601128846af65b343dc4a4b 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -40,9 +40,6 @@ namespace armarx::navigation::server
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
void onLocalPlannerResult(const loc_plan::LocalPlannerResult& result) override;
- void
- onTrajectoryControllerResult(const traj_ctrl::TrajectoryControllerResult& result) override;
- void onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result) override;
void onGoal(const core::Pose& goal) override;
diff --git a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
index bb7e134b08c23e8260e1f4a438b681fd963801a6..7962ed8e8e1b10c8ee82a95d3ab4285d4d122cd5 100644
--- a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
+++ b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.cpp
@@ -42,29 +42,25 @@ namespace armarx::navigation::server
ARMARX_DEBUG << "Orientation goal reached";
}
- std::unique_lock lock{
- scene.platformVelocityMtx}; // thread-safe access to scene.platformVelocity
+ // FIXME:
+ // if (not scene.platformVelocity.has_value())
+ // {
+ // ARMARX_DEBUG << "Platform velocity not available yet.";
+ // return false;
+ // }
- if (not scene.platformVelocity.has_value())
- {
- ARMARX_DEBUG << "Platform velocity not available yet.";
- return false;
- }
-
- const bool linearVelocityLow = scene.platformVelocity->linear.norm() < config.linearVelTh;
- const bool angularVelocityLow =
- scene.platformVelocity->angular.norm() < config.angularVelTh;
+ // const bool linearVelocityLow = scene.platformVelocity->linear.norm() < config.linearVelTh;
+ // const bool angularVelocityLow =
+ // scene.platformVelocity->angular.norm() < config.angularVelTh;
- lock.unlock();
+ // const bool velocityLow = linearVelocityLow and angularVelocityLow;
- const bool velocityLow = linearVelocityLow and angularVelocityLow;
-
- if (velocityLow)
- {
- ARMARX_DEBUG << "Robot has come to rest.";
- }
+ // if (velocityLow)
+ // {
+ // ARMARX_DEBUG << "Robot has come to rest.";
+ // }
- return posReached and oriReached and velocityLow;
+ return posReached and oriReached; //and velocityLow;
}
const core::Pose&
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..804c4d037fd65607755d487288ffa27e430ae52a
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -0,0 +1,106 @@
+#include "SceneProvider.h"
+
+#include <VirtualRobot/SceneObjectSet.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include <armarx/navigation/algorithms/CostmapBuilder.h>
+#include <armarx/navigation/util/util.h>
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ SceneProvider::SceneProvider(const InjectedServices& srv, const Config& config) :
+ srv(srv), config(config)
+ {
+ }
+
+ const core::Scene&
+ SceneProvider::scene() const
+ {
+ return scn;
+ }
+
+ bool
+ SceneProvider::initialize(const DateTime& timestamp)
+ {
+ scn.timestamp = timestamp;
+
+ scn.robot = srv.virtualRobotReader->getRobot(
+ config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
+ ARMARX_CHECK_NOT_NULL(scn.robot);
+
+ scn.staticScene = getStaticScene(timestamp);
+ scn.dynamicScene = getDynamicScene(timestamp);
+ scn.graph = getSceneGraph(timestamp);
+
+ return true; // TODO(fabian.reister): return false if sync fails
+ }
+
+ bool
+ SceneProvider::synchronize(const DateTime& timestamp)
+ {
+ scn.timestamp = timestamp;
+
+ ARMARX_CHECK_NOT_NULL(srv.virtualRobotReader);
+ srv.virtualRobotReader->synchronizeRobot(*scn.robot, timestamp);
+
+ scn.dynamicScene = getDynamicScene(timestamp);
+ // scn.graph = getSceneGraph(timestamp);
+
+ return true; // TODO(fabian.reister): return false if sync fails
+ }
+
+ core::StaticScene
+ SceneProvider::getStaticScene(const DateTime& timestamp) const
+ {
+ ARMARX_TRACE;
+
+ const objpose::ObjectPoseSeq objectPoses = srv.objectPoseClient.fetchObjectPoses();
+
+ // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
+ const auto objectPosesStatic =
+ armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
+
+ const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
+
+ ARMARX_CHECK_NOT_NULL(objects);
+ ARMARX_INFO << objects->getSize() << " objects in the scene";
+
+ ARMARX_INFO << "Creating costmap";
+ ARMARX_CHECK_NOT_NULL(scn.robot);
+
+ // FIXME: move costmap creation out of this component
+ // FIXME create costmap writer enum: type of costmaps
+ algorithms::CostmapBuilder costmapBuilder(
+ scn.robot,
+ objects,
+ algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ "Platform-navigation-colmodel");
+
+ const auto costmap = costmapBuilder.create();
+
+ // ARMARX_INFO << "Storing costmap in memory";
+ // costmapWriterPlugin->get().store(
+ // costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
+
+ ARMARX_INFO << "Done";
+
+ ARMARX_TRACE;
+
+ return {.objects = objects, .costmap = std::make_unique<algorithms::Costmap>(costmap)};
+ }
+
+ core::DynamicScene
+ SceneProvider::getDynamicScene(const DateTime& timestamp) const
+ {
+ return {}; // FIXME implement
+ }
+
+ core::SceneGraph
+ SceneProvider::getSceneGraph(const DateTime& /*timestamp*/) const
+ {
+ ARMARX_CHECK_NOT_NULL(srv.graphReader);
+ return {.subgraphs = srv.graphReader->graphs()};
+ }
+
+} // namespace armarx::navigation::server::scene_provider
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.h b/source/armarx/navigation/server/scene_provider/SceneProvider.h
new file mode 100644
index 0000000000000000000000000000000000000000..cd93e072306246b8dc5a50dc58cee43459e173bf
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.h
@@ -0,0 +1,84 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+#include <armarx/navigation/core/DynamicScene.h>
+#include <armarx/navigation/core/StaticScene.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/graph/Reader.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
+
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ class SceneProvider : virtual public SceneProviderInterface
+ {
+ public:
+ struct InjectedServices
+ {
+
+ memory::client::graph::Reader* graphReader;
+
+ memory::client::costmap::Reader* costmapReader;
+ // armem::vision::occupancy_grid::client::Reader occupancyGridReader;
+
+ // `robot_state` memory reader and writer
+ armem::robot_state::VirtualRobotReader* virtualRobotReader;
+
+ objpose::ObjectPoseClient objectPoseClient;
+ };
+
+ struct Config
+ {
+ std::string robotName;
+ };
+
+ SceneProvider(const InjectedServices& srv, const Config& config);
+
+ const core::Scene& scene() const override;
+
+ bool initialize(const DateTime& timestamp) override;
+ bool synchronize(const DateTime& timestamp) override;
+
+
+ private:
+ VirtualRobot::RobotPtr getRobot(const DateTime& timestamp) const;
+ core::StaticScene getStaticScene(const DateTime& timestamp) const;
+ core::DynamicScene getDynamicScene(const DateTime& timestamp) const;
+ core::SceneGraph getSceneGraph(const DateTime& timestamp) const;
+
+ InjectedServices srv;
+ Config config;
+
+ core::Scene scn;
+ };
+} // namespace armarx::navigation::server::scene_provider
diff --git a/source/armarx/navigation/server/scene_provider/SceneProviderInterface.h b/source/armarx/navigation/server/scene_provider/SceneProviderInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..10869aab3012d42412434560fcbee289ac97d306
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProviderInterface.h
@@ -0,0 +1,46 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+namespace armarx::navigation::core
+{
+ struct Scene;
+}
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ class SceneProviderInterface
+ {
+ public:
+ virtual const core::Scene& scene() const = 0;
+
+ virtual bool initialize(const DateTime& timestamp) = 0;
+ virtual bool synchronize(const DateTime& timestamp) = 0;
+
+
+ // non-api
+ virtual ~SceneProviderInterface() = default;
+ };
+} // namespace armarx::navigation::server::scene_provider
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
index 8252cedb302096ffe3bb8fa4fe42e3f4c5eafd63..be79e84e239d3269a3fe7d69c66ed9cb6364069d 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
@@ -2,8 +2,8 @@
namespace armarx::navigation::traj_ctrl
{
- TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
- {
- }
+ // TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
+ // {
+ // }
} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.h b/source/armarx/navigation/trajectory_control/TrajectoryController.h
index f13e168ade4b91fb653e123c9cf4b2b556ecc87e..72d5927d31f6c8b0885997a25c1aee3ce0ccb853 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryController.h
@@ -61,15 +61,11 @@ namespace armarx::navigation::traj_ctrl
class TrajectoryController
{
public:
- TrajectoryController(const core::Scene& context);
+ TrajectoryController() = default;
virtual ~TrajectoryController() = default;
virtual TrajectoryControllerResult control(const core::Trajectory& goal) = 0;
- protected:
- const core::Scene& context;
-
- private:
};
using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 1997cb8fc631a6d360c04ab70e336541c0bf168f..b7fcdb2df4bdb2278050a16bceb3c425b58dfe81 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -5,12 +5,13 @@
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <VirtualRobot/Robot.h>
-#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
#include <RobotAPI/libraries/core/MultiDimPIDController.h>
+#include "armarx/navigation/core/basic_types.h"
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/trajectory_control/TrajectoryController.h>
@@ -34,7 +35,7 @@ namespace armarx::navigation::traj_ctrl
arondto::TrajectoryFollowingControllerParams dto;
TrajectoryFollowingControllerParams bo;
- aron_conv::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::toAron(dto, bo);
return dto.toAron();
}
@@ -46,16 +47,17 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
TrajectoryFollowingControllerParams bo;
- aron_conv::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::fromAron(dto, bo);
return bo;
}
// TrajectoryFollowingController
- TrajectoryFollowingController::TrajectoryFollowingController(const Params& params,
- const core::Scene& context) :
- TrajectoryController(context),
+ TrajectoryFollowingController::TrajectoryFollowingController(
+ const VirtualRobot::RobotPtr& robot,
+ const Params& params) :
+ robot(robot),
params(params),
pidPos(params.pidPos.Kp,
params.pidPos.Ki,
@@ -93,11 +95,8 @@ namespace armarx::navigation::traj_ctrl
core::Twist
TrajectoryFollowingController::applyTwistLimits(core::Twist twist)
{
- const core::Twist limits
- {
- .linear = Eigen::Vector3f::Ones() * params.limits.linear,
- .angular = Eigen::Vector3f::Ones() * params.limits.angular
- };
+ const core::Twist limits{.linear = Eigen::Vector3f::Ones() * params.limits.linear,
+ .angular = Eigen::Vector3f::Ones() * params.limits.angular};
// for all entries, scale should be less than 1
const auto scalePos = twist.linear.cwiseAbs().cwiseQuotient(limits.linear.cwiseAbs());
@@ -114,13 +113,15 @@ namespace armarx::navigation::traj_ctrl
twist.linear /= scaleMax;
twist.angular /= scaleMax;
+ constexpr float eps = 0.001;
+
// pedantic checks
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular);
- ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular + eps);
+ ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular + eps);
return twist;
}
@@ -128,9 +129,17 @@ namespace armarx::navigation::traj_ctrl
TrajectoryControllerResult
TrajectoryFollowingController::control(const core::Trajectory& trajectory)
{
- ARMARX_CHECK_NOT_NULL(context.robot) << "Robot not available";
+ ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
+
+ if (trajectory.points().empty())
+ {
+ ARMARX_INFO << "Trajectory is empty.";
+ return TrajectoryControllerResult{
+ .twist = core::Twist::Zero(),
+ .dropPoint = {.waypoint = {.pose = core::Pose::Identity()}, .velocity = 0}};
+ }
- const core::Pose currentPose(context.robot->getGlobalPose());
+ const core::Pose currentPose(robot->getGlobalPose());
const auto projectedPose = trajectory.getProjection(
currentPose.translation(), core::VelocityInterpolation::LinearInterpolation);
@@ -182,7 +191,7 @@ namespace armarx::navigation::traj_ctrl
const auto feedforwardVelocity = desiredMovementDirection * ffVel;
ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward direction "
- << feedforwardVelocity.normalized();
+ << feedforwardVelocity.normalized();
ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward velocity " << feedforwardVelocity;
ARMARX_VERBOSE << deactivateSpam(1) << "Control value " << pidPos.getControlValue();
@@ -194,7 +203,17 @@ namespace armarx::navigation::traj_ctrl
ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();
ARMARX_VERBOSE << deactivateSpam(1) << "Twist angular " << twistLimited.angular.transpose();
- return TrajectoryControllerResult{.twist = twistLimited,
+ // convert to the robot's base frame
+ const core::Pose global_T_robot(robot->getGlobalPose());
+
+ const auto& twistGlobal = twistLimited;
+
+ core::Twist twistLocal;
+ twistLocal.linear = global_T_robot.linear().inverse() * twistGlobal.linear;
+ // TODO if not in 2D, then this must be changed!
+ twistLocal.angular = twistGlobal.angular;
+
+ return TrajectoryControllerResult{.twist = twistLocal,
.dropPoint = projectedPose.projection};
}
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
index a9a8a94c46ed3b149d7a48f65e04660a6b5758c6..ffb81732fa4f599c341a3e1528bb791ab4a4395b 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
@@ -48,7 +48,7 @@ namespace armarx::navigation::traj_ctrl
public:
using Params = TrajectoryFollowingControllerParams;
- TrajectoryFollowingController(const Params& params, const core::Scene& context);
+ TrajectoryFollowingController(const VirtualRobot::RobotPtr& robot, const Params& params);
~TrajectoryFollowingController() override = default;
TrajectoryControllerResult control(const core::Trajectory& trajectory) override;
@@ -57,8 +57,10 @@ namespace armarx::navigation::traj_ctrl
core::Twist applyTwistLimits(core::Twist twist);
private:
+ const VirtualRobot::RobotPtr robot;
const Params params;
+
MultiDimPIDControllerTemplate<3> pidPos;
MultiDimPIDControllerTemplate<3> pidPosTarget;
MultiDimPIDControllerTemplate<3> pidOri;
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.cpp b/source/armarx/navigation/trajectory_control/WaypointController.cpp
index c08ff5b97d8e25f5fb5e02934662c0b9436de1b1..527bd4f3a485f233f77db40ff9f55e5d54319117 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.cpp
+++ b/source/armarx/navigation/trajectory_control/WaypointController.cpp
@@ -21,7 +21,7 @@ namespace armarx::navigation::traj_ctrl
arondto::WaypointControllerParams dto;
WaypointControllerParams bo;
- aron_conv::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::toAron(dto, bo);
return dto.toAron();
}
@@ -33,15 +33,15 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
WaypointControllerParams bo;
- aron_conv::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::fromAron(dto, bo);
return bo;
}
// WaypointController
- WaypointController::WaypointController(const Params& params, const core::Scene& context) :
- TrajectoryController(context), params(params)
+ WaypointController::WaypointController(const Params& params) :
+ params(params)
{
}
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.h b/source/armarx/navigation/trajectory_control/WaypointController.h
index 13133e67fbd0a5001fae2451f3fdad0463d34cc4..ec5d4372840eadb710bdd8708fcd829cedc4038e 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.h
+++ b/source/armarx/navigation/trajectory_control/WaypointController.h
@@ -39,7 +39,7 @@ namespace armarx::navigation::traj_ctrl
public:
using Params = WaypointControllerParams;
- WaypointController(const Params& params, const core::Scene& context);
+ WaypointController(const Params& params);
~WaypointController() override = default;
TrajectoryControllerResult control(const core::Trajectory& goal) override;
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.cpp b/source/armarx/navigation/trajectory_control/aron_conversions.cpp
index d8d6d89847b9fcc580874080c457118384e517c5..2cc2d0b42889e5b9323c600f6b0e734b2874f44f 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.cpp
+++ b/source/armarx/navigation/trajectory_control/aron_conversions.cpp
@@ -13,7 +13,7 @@
#include <armarx/navigation/trajectory_control/aron/WaypointControllerParams.aron.generated.h>
-namespace armarx::navigation::traj_ctrl::aron_conv
+namespace armarx::navigation::traj_ctrl
{
void
@@ -67,4 +67,4 @@ namespace armarx::navigation::traj_ctrl::aron_conv
}
-} // namespace armarx::navigation::traj_ctrl::aron_conv
+} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.h b/source/armarx/navigation/trajectory_control/aron_conversions.h
index 14954575365d06862418f2862054df81dc8eeccc..f8496c98a49cf147968a3e02f19a03cd271f6e37 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.h
+++ b/source/armarx/navigation/trajectory_control/aron_conversions.h
@@ -39,10 +39,6 @@ namespace armarx::navigation::traj_ctrl
class WaypointControllerParams;
} // namespace arondto
-} // namespace armarx::navigation::traj_ctrl
-
-namespace armarx::navigation::traj_ctrl::aron_conv
-{
void toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo);
void fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo);
@@ -55,4 +51,4 @@ namespace armarx::navigation::traj_ctrl::aron_conv
void toAron(arondto::WaypointControllerParams& dto, const WaypointControllerParams& bo);
void fromAron(const arondto::WaypointControllerParams& dto, WaypointControllerParams& bo);
-} // namespace armarx::navigation::traj_ctrl::aron_conv
+} // namespace armarx::navigation::traj_ctrl