Commit 135edcb3 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Merge remote-tracking branch 'origin/devel' into devel

parents cb72b669 e9406069
Pipeline #4794 failed with stage
in 36 minutes and 52 seconds
......@@ -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;
......
......@@ -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 ()),
......
......@@ -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
......@@ -804,7 +804,7 @@ namespace hpp {
initProblem ();
pathPlanner_->startSolve ();
pathPlanner_->tryDirectPath ();
pathPlanner_->tryConnectInitAndGoals ();
return roadmap_->pathExists ();
}
......
......@@ -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)
......@@ -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.");
}
......
......@@ -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<ConnectedComponentWkPtr_t> ccs;
......
......@@ -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)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment