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()
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 
92  std::size_t maxSolCount, bool hybridize)
93 {
94  if (!pdef_->getSpaceInformation()->isSetup())
95  pdef_->getSpaceInformation()->setup();
96  foundSolCount_ = 0;
97 
98  time::point start = time::now();
99  std::vector<boost::thread*> threads(planners_.size());
100 
101  // Decide if we are combining solutions or just taking the first one
102  if (hybridize)
103  for (std::size_t i = 0 ; i < threads.size() ; ++i)
104  threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveMore, this, planners_[i].get(), minSolCount, maxSolCount, &ptc));
105  else
106  for (std::size_t i = 0 ; i < threads.size() ; ++i)
107  threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveOne, this, planners_[i].get(), minSolCount, &ptc));
108 
109  for (std::size_t i = 0 ; i < threads.size() ; ++i)
110  {
111  threads[i]->join();
112  delete threads[i];
113  }
114 
115  if (hybridize)
116  {
117  if (phybrid_->pathCount() > 1)
118  if (const base::PathPtr &hsol = phybrid_->getHybridPath())
119  {
120  geometric::PathGeometric *pg = static_cast<geometric::PathGeometric*>(hsol.get());
121  double difference = 0.0;
122  bool approximate = !pdef_->getGoal()->isSatisfied(pg->getStates().back(), &difference);
123  pdef_->addSolutionPath(hsol, approximate, difference, phybrid_->getName()); // name this solution after the hybridization algorithm
124  }
125  }
126 
127  if (pdef_->hasSolution())
128  OMPL_INFORM("ParallelPlan::solve(): Solution found by one or more threads in %f seconds", time::seconds(time::now() - start));
129  else
130  OMPL_WARN("ParallelPlan::solve(): Unable to find solution by any of the threads in %f seconds", time::seconds(time::now() - start));
131 
132  return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
133 }
134 
136 {
137  OMPL_DEBUG("ParallelPlan.solveOne starting planner %s", planner->getName().c_str());
138 
139  time::point start = time::now();
140  if (planner->solve(*ptc))
141  {
142  double duration = time::seconds(time::now() - start);
143  foundSolCountLock_.lock();
144  unsigned int nrSol = ++foundSolCount_;
145  foundSolCountLock_.unlock();
146  if (nrSol >= minSolCount)
147  ptc->terminate();
148  OMPL_DEBUG("ParallelPlan.solveOne: Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
149  }
150 }
151 
152 void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t minSolCount, std::size_t maxSolCount,
154 {
155  OMPL_DEBUG("ParallelPlan.solveMore: starting planner %s", planner->getName().c_str());
156 
157  time::point start = time::now();
158  if (planner->solve(*ptc))
159  {
160  double duration = time::seconds(time::now() - start);
161  foundSolCountLock_.lock();
162  unsigned int nrSol = ++foundSolCount_;
163  foundSolCountLock_.unlock();
164 
165  if (nrSol >= maxSolCount)
166  ptc->terminate();
167 
168  OMPL_DEBUG("ParallelPlan.solveMore: Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
169 
170  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
171 
172  boost::mutex::scoped_lock slock(phlock_);
173  start = time::now();
174  unsigned int attempts = 0;
175  for (std::size_t i = 0 ; i < paths.size() ; ++i)
176  attempts += phybrid_->recordPath(paths[i].path_, false);
177 
178  if (phybrid_->pathCount() >= minSolCount)
179  phybrid_->computeHybridPath();
180 
181  duration = time::seconds(time::now() - start);
182  OMPL_DEBUG("ParallelPlan.solveMore: Spent %f seconds hybridizing %u solution paths (attempted %u connections between paths)", duration,
183  (unsigned int)phybrid_->pathCount(), attempts);
184  }
185 }
void clearHybridizationPaths()
Clear the set of paths recorded for hybrididzation.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
void addPlannerAllocator(const base::PlannerAllocator &pa)
Add a planner allocator to use.
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:422
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...
void clearPlanners()
Clear the set of planners to be executed.
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.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
Base class for a planner.
Definition: Planner.h:232
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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
point now()
Get the current time point.
Definition: Time.h:56
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:60
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns...
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68