RT controller and simplification

The implementation is split such that the Navigator component only generates the trajectory that will subsequently be executed by a RT controller (see: platform_controller/PlatformTrajectory)

Note:

Currently, this cannot be merged as it depends on some unmerged features in armarx::control (branch: new_mp_controller_aron__memory_and_component_plugin__whole_body_control)

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/navigation/common/controller_types.h>
-#include <armarx/navigation/platform_controller/aron/PlatformTrajectoryControllerConfig.aron.generated.h>
-#include <armarx/navigation/platform_controller/aron_conversions.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);
@@ -32,7 +42,6 @@ namespace armarx::navigation::platform_controller::platform_trajectory
             << "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;
 
@@ -55,6 +64,11 @@ namespace armarx::navigation::platform_controller::platform_trajectory
     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
@@ -68,12 +82,39 @@ namespace armarx::navigation::platform_controller::platform_trajectory
         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
@@ -81,16 +122,44 @@ namespace armarx::navigation::platform_controller::platform_trajectory
                           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);
+
+        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);
     }