fix clock issues when start execution after failure

roxanne_rosjava_core/build.gradle

@@ -16,7 +16,7 @@ import sun.security.pkcs.PKCS8Key
  * the License.
  */
 
-version '0.1.26'
+version '0.1.27'
 
 /*
  Dependencies can be on external maven artifacts (such as rosjava_core
@@ -30,6 +30,6 @@ dependencies {
     implementation 'junit:junit:4.12'
     compile 'org.ros.rosjava_core:rosjava:[0.3,0.4)'
     compile 'org.ros.rosjava_messages:roxanne_rosjava_msgs:[0.1.1,)'
-    compile 'it.cnr.istc.pst:platinum:[1.4.4,)'
+    compile 'it.cnr.istc.pst:platinum:[1.4.5,)'
 }
 

roxanne_rosjava_core/src/main/java/com/github/roxanne_rosjava/roxanne_rosjava_core/control/acting/ExecutiveProcess.java

@@ -4,6 +4,7 @@
 import it.cnr.istc.pst.platinum.ai.executive.ExecutiveBuilder;
 import it.cnr.istc.pst.platinum.ai.executive.lang.ex.ExecutionException;
 import it.cnr.istc.pst.platinum.ai.executive.lang.failure.ExecutionFailureCause;
+import it.cnr.istc.pst.platinum.ai.executive.lang.failure.ExecutionFailureCauseType;
 import it.cnr.istc.pst.platinum.ai.executive.pdb.ExecutionNode;
 import it.cnr.istc.pst.platinum.ai.executive.pdb.ExecutionNodeStatus;
 import it.cnr.istc.pst.platinum.ai.framework.microkernel.lang.plan.SolutionPlan;
@@ -86,15 +87,14 @@ public void run() {
 	 * @throws Exception
 	 */
 	protected void doExecute(Goal goal) 
-			throws InterruptedException, ExecutionException, Exception {
+			throws InterruptedException, ExecutionException {
 
 		// get solution plan 
 		SolutionPlan plan = goal.getPlan();
 		// build executive
 		Executive exec = ExecutiveBuilder.createAndSet(this.eClass, 0, plan.getHorizon());
 		// export plan 
 		PlanProtocolDescriptor desc = plan.export();
-		this.log.info("Ready to start (timeline-based) plan execution... ");
 		// set the executive according to the plan being executed
 		exec.initialize(desc);
 
@@ -103,9 +103,21 @@ protected void doExecute(Goal goal)
 			// bind simulator
 			exec.link(this.agent.proxy);
 		}
-
-		// run the executive starting at a given tick
-		boolean complete = exec.execute(goal.getExecutionTick(), goal);
+
+		// start plan execution
+		this.log.info("Ready to start plan execution...\n" +
+				"\t- goal execution tick: " + goal.getExecutionTick() + "\n");
+		// set complete flag
+		boolean complete = false;
+		try {
+
+			// run the executive starting at a given tick
+			complete = exec.execute(goal.getExecutionTick(), goal);
+
+		} catch (Exception ex) {
+			complete = false;
+			this.log.warn("Exception while executing the plan: \n- message: " + ex.getMessage() + "\n");
+		}
 
 		// stop simulator if necessary
 		if (this.agent.proxy != null) {
@@ -120,33 +132,44 @@ protected void doExecute(Goal goal)
 			ExecutionFailureCause cause = exec.getFailureCause();
 			// set failure cause
 			goal.setFailureCause(cause);
-			// set repaired 
-			goal.setRepaired(false);
-			// set goal interruption tick
-			goal.setExecutionTick(cause.getInterruptionTick());
-			// set execution trace by taking into account executed nodes
-			for (ExecutionNode node : exec.getNodes(ExecutionNodeStatus.EXECUTED)) {
-				// add the node to the goal execution trace
-				goal.addNodeToExecutionTrace(node);
-			}
-
-			// get the name of goal components
-			Set<String> components = new HashSet<>();
-			for (TokenDescription t : goal.getTaskDescription().getGoals()) {
-				components.add(t.getComponent());
+			// check failure cause
+			if (cause.equals(ExecutionFailureCauseType.NODE_EXECUTION_ERROR)) {
+
+				// clear executive clock
+				goal.clearExecutionTrace();
+				goal.setExecutionTick(0);
 			}
-
-			// set execution trace by taking into account also (virtual) nodes in-execution
-			for (ExecutionNode node : exec.getNodes(ExecutionNodeStatus.IN_EXECUTION)) {
-				// do not consider nodes belonging to "goal component"
-				if (!components.contains(node.getComponent())) {
+			else {
+
+				// prepare for planning adaptation/re-planning
+				goal.setRepaired(false);
+				// set goal interruption tick
+				goal.setExecutionTick(cause.getInterruptionTick());
+
+				// set execution trace by taking into account executed nodes
+				for (ExecutionNode node : exec.getNodes(ExecutionNodeStatus.EXECUTED)) {
 					// add the node to the goal execution trace
 					goal.addNodeToExecutionTrace(node);
 				}
+
+				// get the name of goal components
+				Set<String> components = new HashSet<>();
+				for (TokenDescription t : goal.getTaskDescription().getGoals()) {
+					components.add(t.getComponent());
+				}
+
+				// set execution trace by taking into account also (virtual) nodes in-execution
+				for (ExecutionNode node : exec.getNodes(ExecutionNodeStatus.IN_EXECUTION)) {
+					// do not consider nodes belonging to "goal component"
+					if (!components.contains(node.getComponent())) {
+						// add the node to the goal execution trace
+						goal.addNodeToExecutionTrace(node);
+					}
+				}
 			}
-			
+
 			// throw exception
-			throw new ExecutionException("Execution failure... try to repair the plan through replanning... \n"
+			throw new ExecutionException("Execution failure... \n"
 					+ "\t- cause: " + cause + "\n", cause);
 		}
 	}

roxanne_rosjava_core/src/main/java/com/github/roxanne_rosjava/roxanne_rosjava_core/control/acting/GoalOrientedActingAgent.java

@@ -1099,19 +1099,21 @@ protected boolean repair(Goal goal)
 
 					// stop execution
 					proceed = false;
-
+					// clear platform
+					this.proxy.clearDispatchedIndex();
 
 					// protect access to the queue
 					synchronized (this.buffered) {
 
 						// set goal status
 						goal.setStatus(GoalStatus.BUFFERED);
+						goal.setExecutionTick(0);
+
 						// add a goal to the queue
 						this.buffered.get(goal.getStatus()).add(goal);
 						// send signal
 						this.buffered.notifyAll();
 					}
-
 				}
 				break;
 
@@ -1243,7 +1245,7 @@ protected boolean repair(Goal goal)
 
 			} else {
 
-				// stop execution
+				// stop execution and make the agent ready to plan again
 				this.status = ActingAgentStatus.READY;
 			}
 

roxanne_rosjava_taskplanner/build.gradle

@@ -33,7 +33,7 @@ mainClassName = 'org.ros.RosRun'
    version number changes.
 */
 
-version '0.1.22'
+version '0.1.23'
 
 dependencies {