diff --git a/src/astar.hh b/src/astar.hh
index 5b4994f3e08849fbfb80020d6e292d1f3934ed2f..28d8285dd86e64f18c113bd273aabbd7d39b166f 100644
--- a/src/astar.hh
+++ b/src/astar.hh
@@ -66,6 +66,7 @@ namespace hpp {
           RoadmapNodePtr_t node = to;
           Nodes_t nodes;
 
+          nodes.push_front (selector_->getNode (to));
           while (node) {
             Parent_t::const_iterator itNode = parent_.find (node);
             if (itNode != parent_.end ()) {
@@ -76,6 +77,8 @@ namespace hpp {
           }
           // We may want to clean it a little
           // std::unique (nodes.begin(), nodes.end ());
+
+          nodes.push_front (selector_->getNode (from_));
           return nodes;
         }
         return Nodes_t();
diff --git a/src/graph/guided-node-selector.cc b/src/graph/guided-node-selector.cc
index 7eb5893734968308346bfdee3decbdbae89d8b62..9bf354dc190dd0fac458735fd9cbddd06af15c8e 100644
--- a/src/graph/guided-node-selector.cc
+++ b/src/graph/guided-node-selector.cc
@@ -71,7 +71,7 @@ namespace hpp {
           Astar alg (roadmap_->distance (), wkPtr_.lock(), from);
           list = alg.solution (static_cast <RoadmapNodePtr_t> (*itg));
         }
-        std::unique (list.begin(), list.end ());
+        list.erase (std::unique (list.begin(), list.end ()), list.end ());
         // Check if the beginning of nodeList is list
         if (list.size() <= nodeList_.size()) {
           Neighbors_t nn;
@@ -94,10 +94,11 @@ namespace hpp {
             for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
               if (it->second->to () == node)
                 nn.insert (it->second, it->first);
-            const Neighbors_t& n1 = (*it1)->neighbors ();
             /// Go from node it1 to node
-            for (Neighbors_t::const_iterator it = n1.begin (); it != n1.end (); ++it)
-              if (it->second->to () == node)
+            // The path will be build from node. So we must find an edge from
+            // node to it1, that will be reversely 
+            for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
+              if (it->second->to () == *it1)
                 nn.insert (it->second, it->first);
           } else {
             Nodes_t::const_iterator it1 = nodeList_.begin ();