From 2041cc01fbb3a2babe720e7c499b441a925192e8 Mon Sep 17 00:00:00 2001
From: Manfred Kroehnert <Manfred.Kroehnert@kit.edu>
Date: Tue, 9 Sep 2014 16:03:32 +0200
Subject: [PATCH] Implement StopRobot XMLState
take code from MotionControl::StopRobot
---
.../MotionControlGroup/StopRobot.cpp | 19 +++++++++++++++++--
1 file changed, 17 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/statecharts/MotionControlGroup/StopRobot.cpp b/source/RobotAPI/statecharts/MotionControlGroup/StopRobot.cpp
index d199bfbca..74e4454f4 100644
--- a/source/RobotAPI/statecharts/MotionControlGroup/StopRobot.cpp
+++ b/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()
--
GitLab