added max age for platform velocity commands. If velocity command is too old, the robot will stop

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp

@@ -40,11 +40,20 @@ NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatfor
     reinitTripleBuffer(initialSettings);
 }
 
-void NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+void NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time&)
 {
-    target->velocityX        = rtGetControlStruct().velocityX;
-    target->velocityY        = rtGetControlStruct().velocityY;
-    target->velocityRotation = rtGetControlStruct().velocityRotation;
+    auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
+
+    if (commandAge > maxCommandDelay) // command must be recent
+    {
+        throw LocalException("platform target velocity was not set for a too long time: delay: ") << commandAge.toSecondsDouble() << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+    }
+    else
+    {
+        target->velocityX        = rtGetControlStruct().velocityX;
+        target->velocityY        = rtGetControlStruct().velocityY;
+        target->velocityRotation = rtGetControlStruct().velocityRotation;
+    }
 }
 
 void NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX, float velocityY,
@@ -54,8 +63,19 @@ void NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(floa
     getWriterControlStruct().velocityX        = velocityX;
     getWriterControlStruct().velocityY        = velocityY;
     getWriterControlStruct().velocityRotation = velocityRotation;
+    getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
     writeControlStruct();
 }
 
+IceUtil::Time NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
+{
+    return maxCommandDelay;
+}
+
+void NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(const IceUtil::Time& value)
+{
+    maxCommandDelay = value;
+}
+
 NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
 registrationNJointHolonomicPlatformUnitVelocityPassThroughController("NJointHolonomicPlatformUnitVelocityPassThroughController");

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h

@@ -48,6 +48,7 @@ namespace armarx
         float velocityX = 0;
         float velocityY = 0;
         float velocityRotation = 0;
+        IceUtil::Time commandTimestamp;
     };
 
     TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
@@ -76,9 +77,13 @@ namespace armarx
         {
             return "NJointHolonomicPlatformUnitVelocityPassThroughController";
         }
+        IceUtil::Time getMaxCommandDelay() const;
+        void setMaxCommandDelay(const IceUtil::Time& value);
+
     protected:
         void onInitComponent() override {}
         void onConnectComponent() override {}
+        IceUtil::Time maxCommandDelay = IceUtil::Time::milliSeconds(500);
 
         ControlTargetHolonomicPlatformVelocity* target;
         NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData initialSettings;