MovePlatform - added a parameter that dictates how long the platform should...

MovePlatform - added a parameter that dictates how long the platform should wait after the last target was reached before going into a success state

scenarios/MovePlatformTest/configs/MovePlatformExample.xml

@@ -40,9 +40,9 @@
             <z>0</z>
         </Item0>
     </targetPositions-->
-    <positionalAccuracy>10</positionAccuracy> <!--mm-->
-    <orientationalAccuracy>0.1</orientationAccuracy> <!--rad-->
-    <timeoutMoveTo>30000</timeoutMoveTO>
+    <positionalAccuracy>10</positionalAccuracy> <!--mm-->
+    <orientationalAccuracy>0.1</orientationalAccuracy> <!--rad-->
+    <timeoutMoveTo>30000</timeoutMoveTo>
   </StateParameters>
 </MovePlatformStateChart>
 

scenarios/MovePlatformTest/configs/MovePlatformExampleCloseToTable.xml

@@ -20,9 +20,12 @@
             <z>0</z>
         </Item0>
     </targetPositions-->
-    <positionalAccuracy>10</positionAccuracy> <!--mm-->
-    <orientationalAccuracy>0.1</orientationAccuracy> <!--rad-->
-    <timeoutMoveTo>30000</timeoutMoveTO>
+    <positionalAccuracy>10</positionalAccuracy> <!--mm-->
+    <orientationalAccuracy>0.1</orientationalAccuracy> <!--rad-->
+    <timeoutMoveTo>30000</timeoutMoveTo> <!--ms-->
+    <!-- after the last target was reached, wait a certain amount of 
+         time (should be smaller than the timeout!) -->
+    <waitAfterLast>10000</waitAfterLast> <!--ms-->
   </StateParameters>
 </MovePlatformStateChart>
 

source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp

@@ -57,6 +57,7 @@ namespace armarx
         addToInput("positionalAccuracy", VariantType::Float, false);
         addToInput("orientationalAccuracy", VariantType::Float, false);
         addToInput("timeoutMoveTo", VariantType::Int, false);
+        addToInput("waitAfterLast", VariantType::Int, false);
         addToLocal("positionCounter", VariantType::ChannelRef);
     }
 
@@ -104,6 +105,7 @@ namespace armarx
         addToInput("positionalAccuracy", VariantType::Float, false);
         addToInput("orientationalAccuracy", VariantType::Float, false);
         addToInput("timeoutMoveTo", VariantType::Int, false);
+        addToInput("waitAfterLast", VariantType::Int, false);
         addToInput("positionCounter", VariantType::ChannelRef, false);
     }
 
@@ -128,7 +130,8 @@ namespace armarx
             condTargetReached = installCondition<EvTargetReached>(checkX && checkY && checkAngle);
             timeoutMoveToNext = setTimeoutEvent(getInput<int>("timeoutMoveTo"), createEvent<EvTimeoutTargetUnreachable>());
         } else {
-            sendEvent<EvNoTargets>();
+//            sendEvent<EvNoTargets>();
+            timeoutWaitAfterLast = setTimeoutEvent(getInput<int>("waitAfterLast"), createEvent<EvNoTargets>());
         }
 
         context->systemObserverPrx->incrementCounter(counter);

source/RobotAPI/statecharts/MovePlatform/MovePlatform.h

@@ -75,6 +75,7 @@ namespace armarx
 
         ConditionIdentifier condTargetReached;
         StateUtility::ActionEventIdentifier timeoutMoveToNext;
+        StateUtility::ActionEventIdentifier timeoutWaitAfterLast;
     };
 
 }