[PathPlanner] Replace tryDirectPath by tryConnectInitAndGoals.

  This new method tries to connect the initial and goal configurations to
  other connected components of the roadmap before developing the roadmap
  further. When planning from an initial to goal configurations without
  precomputed roadmap, this method should be equivalent to tryDirectPath.

include/hpp/core/path-planner.hh

@@ -54,7 +54,9 @@ namespace hpp {
       virtual PathVectorPtr_t solve ();
       /// Try to make direct connection between init and goal
       /// configurations, in order to avoid a random shoot.
-      virtual void tryDirectPath();
+      virtual void tryDirectPath() HPP_CORE_DEPRECATED;
+      /// Try to connect initial and goal configurations to existing roadmap
+      virtual void tryConnectInitAndGoals ();
 
       /// User implementation of one step of resolution
       virtual void oneStep () = 0;

src/path-planner.cc

@@ -16,8 +16,10 @@
 // hpp-core  If not, see
 // <http://www.gnu.org/licenses/>.
 
-#include <hpp/core/path-planner.hh>
+#include <boost/tuple/tuple.hpp>
 
+#include <hpp/core/path-planner.hh>
+#include <hpp/core/nearest-neighbor.hh>
 #include <hpp/util/debug.hh>
 
 #include <hpp/core/roadmap.hh>
@@ -90,10 +92,10 @@ namespace hpp {
       unsigned long int nIter (0);
       boost::posix_time::ptime timeStart(boost::posix_time::microsec_clock::universal_time());
       startSolve ();
-      tryDirectPath ();
+      tryConnectInitAndGoals ();
       solved = problem_.target()->reached (roadmap());
       if (solved ) {
-	hppDout (info, "tryDirectPath succeeded");
+	hppDout (info, "tryConnectInitAndGoals succeeded");
       }
       if (interrupt_) throw std::runtime_error ("Interruption");
       while (!solved) {
@@ -174,5 +176,85 @@ namespace hpp {
         }
       }
     }
+    void PathPlanner::tryConnectInitAndGoals ()
+    {
+      // call steering method here to build a direct conexion
+      const SteeringMethodPtr_t& sm (problem ().steeringMethod ());
+      PathValidationPtr_t pathValidation (problem ().pathValidation ());
+      PathProjectorPtr_t pathProjector (problem ().pathProjector ());
+      PathPtr_t validPath, projPath, path;
+      NodePtr_t initNode = roadmap ()->initNode();
+      NearestNeighborPtr_t nn (roadmap ()->nearestNeighbor ());
+      // Register edges to add to roadmap and add them after iterating
+      // among the connected components.
+      typedef boost::tuple <NodePtr_t, NodePtr_t, PathPtr_t> FutureEdge_t;
+      typedef std::vector <FutureEdge_t> FutureEdges_t;
+      FutureEdges_t futureEdges;
+      ConnectedComponentPtr_t initCC (initNode->connectedComponent ());
+      for (ConnectedComponents_t::iterator itCC
+             (roadmap ()->connectedComponents ().begin ());
+           itCC != roadmap ()->connectedComponents ().end (); ++itCC) {
+        if (*itCC != initCC) {
+          value_type d;
+          NodePtr_t near (nn->search (initNode->configuration (),
+                                      *itCC, d, true));
+          assert (near);
+          ConfigurationPtr_t q1 (initNode->configuration ());
+          ConfigurationPtr_t q2 (near->configuration ());
+          path = (*sm) (*q1, *q2);
+          if (!path) continue;
+          if (pathProjector) {
+            if (!pathProjector->apply (path, projPath)) continue;
+          } else {
+            projPath = path;
+          }
+          if (projPath) {
+            PathValidationReportPtr_t report;
+            bool pathValid = pathValidation->validate (projPath, false,
+                                                       validPath, report);
+            if (pathValid && validPath->length() > 0) {
+              futureEdges.push_back (FutureEdge_t (initNode, near, projPath));
+            }
+          }
+        }
+      }
+      for (NodeVector_t::const_iterator itn = roadmap ()->goalNodes ().begin();
+	   itn != roadmap ()->goalNodes ().end (); ++itn) {
+        ConnectedComponentPtr_t goalCC ((*itn)->connectedComponent ());
+        for (ConnectedComponents_t::iterator itCC
+               (roadmap ()->connectedComponents ().begin ());
+             itCC != roadmap ()->connectedComponents ().end (); ++itCC) {
+          if (*itCC != goalCC) {
+            value_type d;
+            NodePtr_t near (nn->search ((*itn)->configuration (), *itCC, d,
+                                        false));
+            assert (near);
+            ConfigurationPtr_t q1 (near->configuration ());
+            ConfigurationPtr_t q2 ((*itn)->configuration ());
+            path = (*sm) (*q1, *q2);
+            if (!path) continue;
+            if (pathProjector) {
+              if (!pathProjector->apply (path, projPath)) continue;
+            } else {
+              projPath = path;
+            }
+            if (projPath) {
+              PathValidationReportPtr_t report;
+              bool pathValid = pathValidation->validate (projPath, false,
+                                                         validPath, report);
+              if (pathValid && validPath->length() > 0) {
+                futureEdges.push_back (FutureEdge_t (near, (*itn), projPath));
+              }
+            }
+          }
+        }
+      }
+      // Add edges
+      for (FutureEdges_t::const_iterator it (futureEdges.begin ());
+           it != futureEdges.end (); ++it) {
+        roadmap ()->addEdge (it->get <0> (), it->get <1> (), it->get <2> ());
+      }
+    }
+
   } //   namespace core
 } // namespace hpp

src/problem-solver.cc

@@ -809,7 +809,7 @@ namespace hpp {
       initProblem ();
 
       pathPlanner_->startSolve ();
-      pathPlanner_->tryDirectPath ();
+      pathPlanner_->tryConnectInitAndGoals ();
       return roadmap_->pathExists ();
     }