diff --git a/include/hpp/core/path-planner.hh b/include/hpp/core/path-planner.hh
index e478809e8..aa0542de1 100644
--- a/include/hpp/core/path-planner.hh
+++ b/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;
diff --git a/src/path-optimization/simple-shortcut.cc b/src/path-optimization/simple-shortcut.cc
index 43a7bd985..a6edda89b 100644
--- a/src/path-optimization/simple-shortcut.cc
+++ b/src/path-optimization/simple-shortcut.cc
@@ -58,9 +58,7 @@ namespace hpp {
nodes.push_back (node);
}
roadmap->addGoalNode (node->configuration ());
- const SteeringMethod& sm (*(problem ().steeringMethod ()));
PathValidationPtr_t pv (problem ().pathValidation ());
- PathProjectorPtr_t proj (problem ().pathProjector ());
for (std::size_t i=0; i < nodes.size () - 1; ++i) {
for (std::size_t j=i+2; j < nodes.size (); ++j) {
PathPtr_t path (steer (*(nodes [i]->configuration ()),
diff --git a/src/path-planner.cc b/src/path-planner.cc
index 1ed4e03a1..a41620e37 100644
--- a/src/path-planner.cc
+++ b/src/path-planner.cc
@@ -16,8 +16,10 @@
// hpp-core If not, see
// .
-#include
+#include
+#include
+#include
#include
#include
@@ -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 FutureEdge_t;
+ typedef std::vector 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
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index b62856f22..dd7e80b47 100755
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -804,7 +804,7 @@ namespace hpp {
initProblem ();
pathPlanner_->startSolve ();
- pathPlanner_->tryDirectPath ();
+ pathPlanner_->tryConnectInitAndGoals ();
return roadmap_->pathExists ();
}
diff --git a/tests/plugin-test/CMakeLists.txt b/tests/plugin-test/CMakeLists.txt
index 109ee6a33..c0e33ccce 100644
--- a/tests/plugin-test/CMakeLists.txt
+++ b/tests/plugin-test/CMakeLists.txt
@@ -21,4 +21,5 @@ SET_TARGET_PROPERTIES(example PROPERTIES PREFIX ""
TARGET_LINK_LIBRARIES(example ${LIBRARY_NAME})
PKG_CONFIG_USE_DEPENDENCY(example hpp-pinocchio)
+
#INSTALL(TARGETS example DESTINATION lib/hppPlugins)
diff --git a/tests/problem.cc b/tests/problem.cc
index c83c75184..bc5d910bc 100644
--- a/tests/problem.cc
+++ b/tests/problem.cc
@@ -93,7 +93,7 @@ void pointMassProblem (const char* steeringMethod,
ps->solve ();
BOOST_CHECK (ps->roadmap()->nodes().size() > 2);
- BOOST_MESSAGE ("Solved the problem with " << ps->roadmap()->nodes().size()
+ BOOST_TEST_MESSAGE ("Solved the problem with " << ps->roadmap()->nodes().size()
<< " nodes.");
}
@@ -131,7 +131,7 @@ void carLikeProblem (const char* steeringMethod,
ps->solve ();
BOOST_CHECK (ps->roadmap()->nodes().size() > 2);
- BOOST_MESSAGE ("Solved the problem with " << ps->roadmap()->nodes().size()
+ BOOST_TEST_MESSAGE ("Solved the problem with " << ps->roadmap()->nodes().size()
<< " nodes.");
}
diff --git a/tests/roadmap-1.cc b/tests/roadmap-1.cc
index 270ad99ea..df9f7b8fe 100644
--- a/tests/roadmap-1.cc
+++ b/tests/roadmap-1.cc
@@ -126,7 +126,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
nodes.push_back (r->addNode (q));
r->addGoalNode (nodes [5]->configuration ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 6);
for (std::size_t i=0; i < nodes.size (); ++i) {
for (std::size_t j=i+1; j < nodes.size (); ++j) {
@@ -149,7 +149,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
}
BOOST_CHECK (!r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// 1 -> 0
addEdge (r, *sm, nodes, 1, 0);
@@ -166,7 +166,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
}
BOOST_CHECK (!r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// 1 -> 2
addEdge (r, *sm, nodes, 1, 2);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 5);
@@ -181,7 +181,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
}
BOOST_CHECK (!r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// 2 -> 0
addEdge (r, *sm, nodes, 2, 0);
@@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
}
BOOST_CHECK (!r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// 2 -> 3
addEdge (r, *sm, nodes, 2, 3);
@@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
}
BOOST_CHECK (!r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// 2 -> 4
addEdge (r, *sm, nodes, 2, 4);
@@ -235,7 +235,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
}
BOOST_CHECK (!r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// 3 -> 5
addEdge (r, *sm, nodes, 3, 5);
@@ -253,7 +253,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
}
BOOST_CHECK (r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// 4 -> 5
addEdge (r, *sm, nodes, 4, 5);
@@ -271,7 +271,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
}
BOOST_CHECK (r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// 5 -> 0
addEdge (r, *sm, nodes, 5, 0);
@@ -287,7 +287,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
BOOST_CHECK (nodes [0]->connectedComponent () ==
nodes [5]->connectedComponent ());
BOOST_CHECK (r->pathExists ());
- BOOST_MESSAGE(*r);
+ BOOST_TEST_MESSAGE(*r);
// Check that memory if well deallocated.
std::set ccs;
diff --git a/tests/test-intervals.cc b/tests/test-intervals.cc
index 9a6fd1b96..ee43a3eec 100644
--- a/tests/test-intervals.cc
+++ b/tests/test-intervals.cc
@@ -242,7 +242,7 @@ BOOST_AUTO_TEST_CASE (interval_6)
intervals.unionInterval (interval);
checkIntervals (intervals);
- BOOST_MESSAGE(intervals);
+ BOOST_TEST_MESSAGE(intervals);
}
BOOST_AUTO_TEST_CASE (interval_7)