ADStarForward support for replanning

hipster-core/src/main/java/es/usc/citius/hipster/algorithm/ADStarForward.java

@@ -17,13 +17,7 @@
 package es.usc.citius.hipster.algorithm;
 
 import es.usc.citius.hipster.model.Transition;
-import es.usc.citius.hipster.model.function.CostFunction;
-import es.usc.citius.hipster.model.function.HeuristicFunction;
-import es.usc.citius.hipster.model.function.NodeFactory;
-import es.usc.citius.hipster.model.function.TransitionFunction;
 import es.usc.citius.hipster.model.function.impl.*;
-import es.usc.citius.hipster.model.impl.ADStarNodeImpl;
-import es.usc.citius.hipster.model.problem.SearchComponents;
 
 import java.util.ArrayList;
 import java.util.Collection;
@@ -106,12 +100,14 @@ public class Iterator implements java.util.Iterator<N> {
         protected Map<S, N> open;
         protected Map<S, N> closed;
         protected Map<S, N> incons;
-        protected Iterable<Transition<A, S>> transitionsChanged;
+        protected Collection<Transition<A, S>> transitionsChanged;
         protected Queue<N> queue;
+        protected boolean replan;
         protected final N beginNode;
         protected final Collection<N> goalNodes;
 
         protected Iterator() {
+            this.replan = false;
             //initialize nodes
             this.beginNode = expander.makeNode(null, new Transition<A, S>(null, begin));
             //initialize goal node collection
@@ -192,14 +188,20 @@ protected void updateQueues(N node) {
         }
 
         /**
-         * As the algorithm is executed iteratively refreshing the changed relations
-         * between nodes, this method will return always true.
+         * The iterator will have next() nodes when the stop condition of the algorithm is not reached, or if the
+         * value of Epsilon has changed and a replanning is needed. It may happen that after chaning Epsilon the
+         * solution does not improve, and therefore this method will return false. In that case, Epsilon should be
+         * reduced again until the minimum value of 1 is reached. In that case, the solution will be optimal.
          *
-         * @return always true
+         * @see #setEpsilon(double)
+         *
+         * @return true if a replan is pending or the solution has not been found
          */
         @Override
         public boolean hasNext() {
-            return takePromising() != null;
+            N current = takePromising();
+            N minGoal = Collections.min(goalNodes);
+            return replan || minGoal.compareTo(current) >= 0 || minGoal.getV().compareTo(minGoal.getG()) < 0;
         }
 
         /**
@@ -238,19 +240,28 @@ public N next() {
                     updateQueues(current);
                 }
             } else {
+                this.replan = false;
                 // for all directed edges (u, v) with changed edge costs
-                for(N nodeTransitionsChanged : expander.expandTransitionsChanged(beginNode.state(), current, transitionsChanged)){
+                for(N nodeTransitionsChanged : expander.expandTransitionsChanged(beginNode, transitionsChanged)){
                     updateQueues(nodeTransitionsChanged);
                 }
+                //empty the list of transitions
+                transitionsChanged.clear();
                 //move states from INCONS to OPEN
                 open.putAll(incons);
+                //empty INCONS queue
+                incons.clear();
                 //updateQueues the priorities for all s in OPEN according to key(s)
                 queue.clear();
                 for(N node : open.values()){
+                    //key is recalculated according to the new value of Epsilon
+                    expander.updateKey(node);
+                    //insert into the priority queue
                     queue.offer(node);
                 }
                 //closed = empty
                 closed.clear();
+                current = takePromising();
             }
             return current;
         }
@@ -274,6 +285,40 @@ public N next() {
         public Map<S, N> getClosed() { return closed; }
 
         public Map<S, N> getIncons() { return incons; }
+
+        /**
+         * Updates the value of epsilon to improve the cost of the solutions.
+         * The update of Epsilon must be manually done, as this parameter higlhy depends on the heuristic used,
+         * and the search problem. Use only values of epsilon above 1, as the opposite will lead to diminish the
+         * estimate of the heuristic, which is supposed to be optimistic. Values below 1 result in underestimating
+         * the cost to the goal, and therefore in a greater number of expansions to find the same solution than with
+         * epsilon = 1.
+         *
+         * @param epsilon new value of epsilon (sub-optimal bound to obtain anytime solutions)
+         */
+        public void setEpsilon(double epsilon){
+            this.replan = true;
+            expander.setEpsilon(epsilon);
+        }
+
+        /**
+         * Queries the current value of Epsilon (sub-optimal bound for anytime solutions).
+         *
+         * @return current value of Epsilon
+         */
+        public double getEpsilon(){
+            return expander.getEpsilon();
+        }
+
+        /**
+         * Marks transitions to be processed in the next replan event.
+         *
+         * @param transitions
+         */
+        public void addTransitionsChanged(Collection<Transition<A, S>> transitions){
+            this.replan = true;
+            transitionsChanged.addAll(transitions);
+        }
     }
 
 }

hipster-core/src/main/java/es/usc/citius/hipster/model/function/impl/ADStarNodeExpander.java

@@ -129,16 +129,16 @@ public Iterable<N> expand(N node) {
      * @param transitions list of transitions with changed costs
      * @return list of updated nodes
      */
-    public Iterable<N> expandTransitionsChanged(S begin, N current, Iterable<Transition<A, S>> transitions){
+    public Iterable<N> expandTransitionsChanged(N begin, Iterable<Transition<A, S>> transitions){
         Collection<N> nodes = new ArrayList<N>();
         for (Transition<A, S> transition : transitions) {
             S state = transition.getState();
             //if v != start
-            if (!state.equals(begin)) {
+            if (!state.equals(begin.state())) {
                 //if s' not visited before: v(s')=g(s')=Infinity; bp(s')=null
                 N node = this.visited.get(state);
                 if (node == null) {
-                    node = nodeFactory.makeNode(current, transition);
+                    node = nodeFactory.makeNode(begin, transition);
                     visited.put(state, node);
                 }
                 // bp(v) = arg min s'' predecessor of v such that (v(s'') + c(s'', v))
@@ -240,6 +240,15 @@ public void setMaxV(N node) {
         node.setV(this.add.getMaxElem());
     }
 
+    /**
+     * Assigns the maximum value to G in the current node.
+     *
+     * @param node {@link es.usc.citius.hipster.model.impl.ADStarNodeImpl} to modify the value of V
+     */
+    public void setMaxG(N node)  {
+        node.setG(this.add.getMaxElem());
+    }
+
     /**
      * Assign a value to the inflation parameter of the heuristic.
      *
@@ -249,6 +258,15 @@ public void setEpsilon(double epsilon) {
         this.epsilon = epsilon;
     }
 
+    /**
+     * Queries the current value of epsilon (sub-optimal bound for anytime solutions).
+     *
+     * @return current value of epsilon
+     */
+    public double getEpsilon() {
+        return epsilon;
+    }
+
     /**
      * @return map with the states and nodes visited by the algorithm
      */
@@ -269,4 +287,16 @@ public void setEpsilon(double epsilon) {
     public N makeNode(N from, Transition<A, S> transition){
         return nodeFactory.makeNode(from, transition);
     }
+
+    /**
+     * Updating the priority of a node is required when changing the value of Epsilon.
+     */
+    public void updateKey(N node){
+        node.setKey(new es.usc.citius.hipster.model.ADStarNode.Key<C>(node.getG(), node.getV(),
+                heuristicFunction.estimate(node.state()), epsilon, add, scale));
+    }
+
+    public void setMaxKey(N node){
+        node.setKey(new ADStarNode.Key<C>(add.getMaxElem(), add.getMaxElem()));
+    }
 }