From b5538c0c6162388270f4487c124dcc1c8f213dcd Mon Sep 17 00:00:00 2001
From: Mirko Waechter <waechter@kit.edu>
Date: Sun, 3 Aug 2014 00:23:41 +0200
Subject: [PATCH] Hopefully fixed #1310
---
.../RobotAPI/motioncontrol/MotionControl.cpp | 25 ++++++++-----------
source/RobotAPI/motioncontrol/MotionControl.h | 1 +
2 files changed, 12 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index 14d506848..4804e2bfe 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -324,6 +324,11 @@ void MoveTCPTrajectory::onEnter()
throw armarx::exceptions::local::eStatechartLogicError("the number of targetTCPPositions does not equal the number of targetTCPOrientations");
sendEvent<EvFailure>();
}
+ // create a counter to keep track of the poses that were already reached. As we dont use the event, set the max to n+1 so it will never be sent
+
+ ARMARX_INFO << "number of points on trajectory: " << numberOfPoints;
+ trajectoryPointCounterID = setCounterEvent(numberOfPoints, createEvent<EvSuccess>());
+ setLocal("trajectoryPointCounterID", Variant(trajectoryPointCounterID.actionId));
}
@@ -331,9 +336,7 @@ void MoveTCPTrajectory::onEnter()
void MoveTCPTrajectory::onExit()
{
removeTimeoutEvent(timeoutEvent);
- ChannelRefPtr r = getLocal<ChannelRef>("trajectoryPointCounterID");
- getContext()->systemObserverPrx->removeCounter(r);
- //removeCounterEvent(trajectoryPointCounterID);
+ removeCounterEvent(trajectoryPointCounterID);
}
@@ -359,8 +362,8 @@ void MoveTCPTrajectory::defineSubstates()
void MoveTCPTrajectoryInit::defineParameters()
{
- addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
- addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false);
+// addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
+// addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false);
}
@@ -368,11 +371,7 @@ void MoveTCPTrajectoryInit::defineParameters()
void MoveTCPTrajectoryInit::onEnter()
{
- // create a counter to keep track of the poses that were already reached. As we dont use the event, set the max to n+1 so it will never be sent
- int numberOfPoints = getInput<SingleTypeVariantList>("targetTCPPositions")->getSize();
- ARMARX_INFO << "number of points on trajectory: " << numberOfPoints;
- ActionEventIdentifier trajectoryPointCounterID = setCounterEvent(numberOfPoints, createEvent<EvLastPointReached>());
- setOutput("trajectoryPointCounterID", trajectoryPointCounterID.actionId.get());
+
sendEvent<EvSuccess>();
}
@@ -384,7 +383,7 @@ void MoveTCPTrajectoryCheckCounter::defineParameters()
{
addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
addToInput("trajectoryPointCounterID", VariantType::ChannelRef, false);
- addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false);
+
}
@@ -394,7 +393,6 @@ void MoveTCPTrajectoryCheckCounter::onEnter()
{
context = getContext<RobotStatechartContext>();
ChannelRefPtr trajectoryPointCounterID = getInput<ChannelRef>("trajectoryPointCounterID");
- setOutput("trajectoryPointCounterID", trajectoryPointCounterID);
int counterValue = trajectoryPointCounterID->getDataField("value")->getInt();
@@ -427,7 +425,6 @@ void MoveTCPTrajectoryNextPoint::defineParameters()
addToInput("timeoutInMs", VariantType::Int, false);
addToInput("trajectoryPointCounterID", VariantType::ChannelRef, false);
- addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false);
addToLocal("targetTCPPosition", VariantType::FramedPosition);
@@ -444,7 +441,6 @@ void MoveTCPTrajectoryNextPoint::onEnter()
{
context = getContext<RobotStatechartContext>();
ChannelRefPtr trajectoryPointCounterID = getInput<ChannelRef>("trajectoryPointCounterID");
- setOutput("trajectoryPointCounterID", trajectoryPointCounterID);
int counterValue = trajectoryPointCounterID->getDataField("value")->getInt();
@@ -452,6 +448,7 @@ void MoveTCPTrajectoryNextPoint::onEnter()
setLocal("targetTCPOrientation", *VariantPtr::dynamicCast(getInput<SingleTypeVariantList>("targetTCPOrientations")->getVariant(counterValue)));
context->systemObserverPrx->incrementCounter(trajectoryPointCounterID);
+
}
diff --git a/source/RobotAPI/motioncontrol/MotionControl.h b/source/RobotAPI/motioncontrol/MotionControl.h
index c629105c0..494afd727 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.h
+++ b/source/RobotAPI/motioncontrol/MotionControl.h
@@ -202,6 +202,7 @@ namespace MotionControl
RobotStatechartContext* context;
ConditionIdentifier lastTargetReachedCondition;
ActionEventIdentifier timeoutEvent;
+ ActionEventIdentifier trajectoryPointCounterID;
void defineParameters();
void defineSubstates();
void onEnter();
--
GitLab