All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
ParallelPlan.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/tools/multiplan/ParallelPlan.h"
38 #include "ompl/geometric/PathHybridization.h"
39 
41  pdef_(pdef), phybrid_(new geometric::PathHybridization(pdef->getSpaceInformation()))
42 {
43 }
44 
45 ompl::tools::ParallelPlan::~ParallelPlan(void)
46 {
47 }
48 
50 {
51  if (planner && planner->getSpaceInformation().get() != pdef_->getSpaceInformation().get())
52  throw Exception("Planner instance does not match space information");
53  if (planner->getProblemDefinition().get() != pdef_.get())
54  planner->setProblemDefinition(pdef_);
55  planners_.push_back(planner);
56 }
57 
59 {
60  base::PlannerPtr planner = pa(pdef_->getSpaceInformation());
61  planner->setProblemDefinition(pdef_);
62  planners_.push_back(planner);
63 }
64 
66 {
67  planners_.clear();
68 }
69 
71 {
72  phybrid_->clear();
73 }
74 
76 {
77  return solve(solveTime, 1, planners_.size(), hybridize);
78 }
79 
80 ompl::base::PlannerStatus ompl::tools::ParallelPlan::solve(double solveTime, std::size_t minSolCount, std::size_t maxSolCount, bool hybridize)
81 {
82  return solve(base::timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)), minSolCount, maxSolCount, hybridize);
83 }
84 
85 
87 {
88  return solve(ptc, 1, planners_.size(), hybridize);
89 }
90 
91 ompl::base::PlannerStatus ompl::tools::ParallelPlan::solve(const base::PlannerTerminationCondition &ptc, std::size_t minSolCount, std::size_t maxSolCount, bool hybridize)
92 {
93  if (!pdef_->getSpaceInformation()->isSetup())
94  pdef_->getSpaceInformation()->setup();
95  foundSolCount_ = 0;
96 
97  time::point start = time::now();
98  std::vector<boost::thread*> threads(planners_.size());
99  if (hybridize)
100  for (std::size_t i = 0 ; i < threads.size() ; ++i)
101  threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveMore, this, planners_[i].get(), minSolCount, maxSolCount, &ptc));
102  else
103  for (std::size_t i = 0 ; i < threads.size() ; ++i)
104  threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveOne, this, planners_[i].get(), minSolCount, &ptc));
105 
106  for (std::size_t i = 0 ; i < threads.size() ; ++i)
107  {
108  threads[i]->join();
109  delete threads[i];
110  }
111 
112  if (hybridize)
113  {
114  if (phybrid_->pathCount() > 1)
115  if (const base::PathPtr &hsol = phybrid_->getHybridPath())
116  {
117  geometric::PathGeometric *pg = static_cast<geometric::PathGeometric*>(hsol.get());
118  double difference = 0.0;
119  bool approximate = !pdef_->getGoal()->isSatisfied(pg->getStates().back(), &difference);
120  pdef_->addSolutionPath(hsol, approximate, difference);
121  }
122  }
123 
124  OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
125 
126  return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
127 }
128 
130 {
131  OMPL_DEBUG("Starting %s", planner->getName().c_str());
132  time::point start = time::now();
133  if (planner->solve(*ptc))
134  {
135  double duration = time::seconds(time::now() - start);
136  foundSolCountLock_.lock();
137  unsigned int nrSol = ++foundSolCount_;
138  foundSolCountLock_.unlock();
139  if (nrSol >= minSolCount)
140  ptc->terminate();
141  OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
142  }
143 }
144 
145 void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t minSolCount, std::size_t maxSolCount, const base::PlannerTerminationCondition *ptc)
146 {
147  time::point start = time::now();
148  if (planner->solve(*ptc))
149  {
150  double duration = time::seconds(time::now() - start);
151  foundSolCountLock_.lock();
152  unsigned int nrSol = ++foundSolCount_;
153  foundSolCountLock_.unlock();
154 
155  if (nrSol >= maxSolCount)
156  ptc->terminate();
157 
158  OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
159 
160  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
161 
162  boost::mutex::scoped_lock slock(phlock_);
163  start = time::now();
164  unsigned int attempts = 0;
165  for (std::size_t i = 0 ; i < paths.size() ; ++i)
166  attempts += phybrid_->recordPath(paths[i].path_, false);
167 
168  if (phybrid_->pathCount() >= minSolCount)
169  phybrid_->computeHybridPath();
170 
171  duration = time::seconds(time::now() - start);
172  OMPL_DEBUG("Spent %f seconds hybridizing %u solution paths (attempted %u connections between paths)", duration, (unsigned int)phybrid_->pathCount(), attempts);
173  }
174 }
void clearHybridizationPaths(void)
Clear the set of paths recorded for hybrididzation.
void clearPlanners(void)
Clear the set of planners to be executed.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
void addPlannerAllocator(const base::PlannerAllocator &pa)
Add a planner allocator to use.
const std::string & getName(void) const
Get the name of the planner.
Definition: Planner.cpp:55
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:417
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void solveMore(base::Planner *planner, std::size_t minSolCount, std::size_t maxSolCount, const base::PlannerTerminationCondition *ptc)
Run the planner and collect the solutions. This function is only called if hybridize_ is true...
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
void terminate(void) const
Notify that the condition for termination should become true, regardless of what eval() returns...
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
base::PlannerStatus solve(double solveTime, bool hybridize=true)
Call Planner::solve() for all planners, in parallel, each planner running for at most solveTime secon...
A boost shared pointer wrapper for ompl::base::Planner.
ParallelPlan(const base::ProblemDefinitionPtr &pdef)
Create an instance for a specified space information.
Base class for a planner.
Definition: Planner.h:227
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
void addPlanner(const base::PlannerPtr &planner)
Add a planner to use.
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
The exception type for ompl.
Definition: Exception.h:47
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
void solveOne(base::Planner *planner, std::size_t minSolCount, const base::PlannerTerminationCondition *ptc)
Run the planner and call ompl::base::PlannerTerminationCondition::terminate() for the other planners ...
Definition of a geometric path.
Definition: PathGeometric.h:55
point now(void)
Get the current time point.
Definition: Time.h:56
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68