scenarios/PlatformNavigationDynamic/config/global.cfg

+# ==================================================================
+# Global Config from Scenario PlatformNavigationDynamic
+# ==================================================================
+

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)

source/armarx/navigation/algorithms/aron_conversions.cpp

 #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

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>
 

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

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>

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>

source/armarx/navigation/common/controller_types.h

+#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

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>

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()
     {

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;

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>
 

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.

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>
 
 

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

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

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

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)
         {
         }

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>
 

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