Hopefully fixed #1310

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);
+
 }
 
 

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();