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

source/armarx/navigation/core/time/ChronoMonotonicTimeServer.cpp

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

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

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

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;
         }
 

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:

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);
 

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;

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;

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
 {

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:

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
 {

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
 {

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
 {

source/armarx/navigation/memory/client/costmap/Writer.cpp

 #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

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
 {

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>

source/armarx/navigation/memory/client/parameterization/Reader.cpp

 #include "Reader.h"
 
-#include "armarx/navigation/memory/constants.h"
+#include <armarx/navigation/memory/constants.h>
 
 namespace armarx::navigation::memory::client::param
 {

source/armarx/navigation/platform_controller/CMakeLists.txt

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

source/armarx/navigation/platform_controller/PlatformTrajectoryController.cpp

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