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 #include <thread>
40 
41 ompl::tools::ParallelPlan::ParallelPlan(const base::ProblemDefinitionPtr &pdef)
42  : pdef_(pdef), phybrid_(std::make_shared<geometric::PathHybridization>(pdef->getSpaceInformation()))
43 {
44 }
45 
46 ompl::tools::ParallelPlan::~ParallelPlan() = default;
47 
48 void ompl::tools::ParallelPlan::addPlanner(const base::PlannerPtr &planner)
49 {
50  if (planner && planner->getSpaceInformation().get() != pdef_->getSpaceInformation().get())
51  throw Exception("Planner instance does not match space information");
52  if (planner->getProblemDefinition().get() != pdef_.get())
53  planner->setProblemDefinition(pdef_);
54  planners_.push_back(planner);
55 }
56 
58 {
59  base::PlannerPtr planner = pa(pdef_->getSpaceInformation());
60  planner->setProblemDefinition(pdef_);
61  planners_.push_back(planner);
62 }
63 
65 {
66  planners_.clear();
67 }
68 
70 {
71  phybrid_->clear();
72 }
73 
75 {
76  return solve(solveTime, 1, planners_.size(), hybridize);
77 }
78 
79 ompl::base::PlannerStatus ompl::tools::ParallelPlan::solve(double solveTime, std::size_t minSolCount,
80  std::size_t maxSolCount, bool hybridize)
81 {
82  return solve(base::timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)), minSolCount,
83  maxSolCount, hybridize);
84 }
85 
87 {
88  return solve(ptc, 1, planners_.size(), hybridize);
89 }
90 
92  std::size_t minSolCount, std::size_t maxSolCount,
93  bool hybridize)
94 {
95  if (!pdef_->getSpaceInformation()->isSetup())
96  pdef_->getSpaceInformation()->setup();
97  foundSolCount_ = 0;
98 
99  time::point start = time::now();
100  std::vector<std::thread *> threads(planners_.size());
101 
102  // Decide if we are combining solutions or just taking the first one
103  if (hybridize)
104  for (std::size_t i = 0; i < threads.size(); ++i)
105  threads[i] = new std::thread([this, i, minSolCount, maxSolCount, &ptc]
106  {
107  solveMore(planners_[i].get(), minSolCount, maxSolCount, &ptc);
108  });
109  else
110  for (std::size_t i = 0; i < threads.size(); ++i)
111  threads[i] = new std::thread([this, i, minSolCount, &ptc]
112  {
113  solveOne(planners_[i].get(), minSolCount, &ptc);
114  });
115 
116  for (auto &thread : threads)
117  {
118  thread->join();
119  delete thread;
120  }
121 
122  if (hybridize)
123  {
124  if (phybrid_->pathCount() > 1)
125  if (const geometric::PathGeometricPtr &hsol = phybrid_->getHybridPath())
126  {
127  double difference = 0.0;
128  bool approximate = !pdef_->getGoal()->isSatisfied(hsol->getStates().back(), &difference);
129  pdef_->addSolutionPath(hsol, approximate, difference,
130  phybrid_->getName()); // name this solution after the hybridization algorithm
131  }
132  }
133 
134  if (pdef_->hasSolution())
135  OMPL_INFORM("ParallelPlan::solve(): Solution found by one or more threads in %f seconds",
136  time::seconds(time::now() - start));
137  else
138  OMPL_WARN("ParallelPlan::solve(): Unable to find solution by any of the threads in %f seconds",
139  time::seconds(time::now() - start));
140 
141  return {pdef_->hasSolution(), pdef_->hasApproximateSolution()};
142 }
143 
144 void ompl::tools::ParallelPlan::solveOne(base::Planner *planner, std::size_t minSolCount,
146 {
147  OMPL_DEBUG("ParallelPlan.solveOne starting planner %s", planner->getName().c_str());
148 
149  time::point start = time::now();
150  try
151  {
152  if (planner->solve(*ptc))
153  {
154  double duration = time::seconds(time::now() - start);
155  foundSolCountLock_.lock();
156  unsigned int nrSol = ++foundSolCount_;
157  foundSolCountLock_.unlock();
158  if (nrSol >= minSolCount)
159  ptc->terminate();
160  OMPL_DEBUG("ParallelPlan.solveOne: Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
161  }
162  }
163  catch (Exception &e)
164  {
165  OMPL_ERROR("Exception thrown during ParrallelPlan::solveOne: %s", e.what());
166  }
167 }
168 
169 void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t minSolCount, std::size_t maxSolCount,
171 {
172  OMPL_DEBUG("ParallelPlan.solveMore: starting planner %s", planner->getName().c_str());
173 
174  time::point start = time::now();
175  try
176  {
177  if (planner->solve(*ptc))
178  {
179  double duration = time::seconds(time::now() - start);
180  foundSolCountLock_.lock();
181  unsigned int nrSol = ++foundSolCount_;
182  foundSolCountLock_.unlock();
183 
184  if (nrSol >= maxSolCount)
185  ptc->terminate();
186 
187  OMPL_DEBUG("ParallelPlan.solveMore: Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
188 
189  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
190 
191  std::lock_guard<std::mutex> slock(phlock_);
192  start = time::now();
193  unsigned int attempts = 0;
194  for (const auto &path : paths)
195  attempts += phybrid_->recordPath(std::static_pointer_cast<geometric::PathGeometric>(path.path_), false);
196 
197  if (phybrid_->pathCount() >= minSolCount)
198  phybrid_->computeHybridPath();
199 
200  duration = time::seconds(time::now() - start);
201  OMPL_DEBUG("ParallelPlan.solveMore: Spent %f seconds hybridizing %u solution paths (attempted %u connections "
202  "between paths)",
203  duration, (unsigned int)phybrid_->pathCount(), attempts);
204  }
205  }
206  catch (Exception &e)
207  {
208  OMPL_ERROR("Exception thrown during ParrallelPlan::solveMore: %s", e.what());
209  }
210 }
Base class for a planner.
Definition: Planner.h:279
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
void clearPlanners()
Clear the set of planners to be executed.
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
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 addPlannerAllocator(const base::PlannerAllocator &pa)
Add a planner allocator to use.
point now()
Get the current time point.
Definition: Time.h:122
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A class to store the exit status of Planner::solve()
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:501
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 ...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
ParallelPlan(const base::ProblemDefinitionPtr &pdef)
Create an instance for a specified space information.
void clearHybridizationPaths()
Clear the set of paths recorded for hybrididzation.
void addPlanner(const base::PlannerPtr &planner)
Add a planner to use.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
The exception type for ompl.
Definition: Exception.h:78
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
base::PlannerStatus solve(double solveTime, bool hybridize=true)
Call Planner::solve() for all planners, in parallel, each planner running for at most solveTime secon...