Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/devel' into devel
Browse files Browse the repository at this point in the history
  • Loading branch information
@jmirabel
jmirabel committed Mar 28, 2019
2 parents cb72b66 + e940606 commit 135edcb
Show file tree
Hide file tree
Showing 8 changed files with 103 additions and 20 deletions.
4 changes: 3 additions & 1 deletion include/hpp/core/path-planner.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 0 additions & 2 deletions src/path-optimization/simple-shortcut.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ()),
Expand Down
88 changes: 85 additions & 3 deletions src/path-planner.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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
2 changes: 1 addition & 1 deletion src/problem-solver.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -804,7 +804,7 @@ namespace hpp {
initProblem ();

pathPlanner_->startSolve ();
pathPlanner_->tryDirectPath ();
pathPlanner_->tryConnectInitAndGoals ();
return roadmap_->pathExists ();
}

Expand Down
1 change: 1 addition & 0 deletions tests/plugin-test/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
4 changes: 2 additions & 2 deletions tests/problem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.");
}

Expand Down Expand Up @@ -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.");
}

Expand Down
20 changes: 10 additions & 10 deletions tests/roadmap-1.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion tests/test-intervals.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit 135edcb

Please sign in to comment.