Implement StopRobot XMLState

take code from MotionControl::StopRobot

source/RobotAPI/statecharts/MotionControlGroup/StopRobot.cpp

@@ -23,6 +23,8 @@
 
 #include "StopRobot.h"
 
+#include <RobotAPI/libraries/core/RobotStatechartContext.h>
+
 using namespace armarx;
 using namespace MotionControlGroup;
 
@@ -38,8 +40,21 @@ StopRobot::StopRobot(XMLStateConstructorParams stateData) :
 
 void StopRobot::onEnter()
 {
-    // put your user code for the enter-point here
-    // execution time should be short (<100ms)
+    ARMARX_LOG << "entering MotionControl::StopRobot" << flush;
+
+    RobotStatechartContext* context = getContext<RobotStatechartContext>();
+
+    armarx::NameList allNodes = context->robotStateComponent->getRobotSnapshot("StopRobotTime")->getRobotNodes();
+
+    NameValueMap jointVelocities;
+    for (const std::string& nodeName : allNodes)
+    {
+        jointVelocities[nodeName] = 0.0f;
+    }
+
+    context->kinematicUnitPrx->begin_setJointVelocities(jointVelocities);
+
+    sendEvent<EvSuccess>();
 }
 
 void StopRobot::run()