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 
42  : pdef_(pdef), phybrid_(std::make_shared<geometric::PathHybridization>(pdef->getSpaceInformation()))
43 {
44 }
45 
46 ompl::tools::ParallelPlan::~ParallelPlan() = default;
47 
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, maxSolCount, &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 base::PathPtr &hsol = phybrid_->getHybridPath())
126  {
127  auto *pg = static_cast<geometric::PathGeometric *>(hsol.get());
128  double difference = 0.0;
129  bool approximate = !pdef_->getGoal()->isSatisfied(pg->getStates().back(), &difference);
130  pdef_->addSolutionPath(hsol, approximate, difference,
131  phybrid_->getName()); // name this solution after the hybridization algorithm
132  }
133  }
134 
135  if (pdef_->hasSolution())
136  OMPL_INFORM("ParallelPlan::solve(): Solution found by one or more threads in %f seconds",
137  time::seconds(time::now() - start));
138  else
139  OMPL_WARN("ParallelPlan::solve(): Unable to find solution by any of the threads in %f seconds",
140  time::seconds(time::now() - start));
141 
142  return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
143 }
144 
145 void ompl::tools::ParallelPlan::solveOne(base::Planner *planner, std::size_t minSolCount,
147 {
148  OMPL_DEBUG("ParallelPlan.solveOne starting planner %s", planner->getName().c_str());
149 
150  time::point start = time::now();
151  if (planner->solve(*ptc))
152  {
153  double duration = time::seconds(time::now() - start);
154  foundSolCountLock_.lock();
155  unsigned int nrSol = ++foundSolCount_;
156  foundSolCountLock_.unlock();
157  if (nrSol >= minSolCount)
158  ptc->terminate();
159  OMPL_DEBUG("ParallelPlan.solveOne: Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
160  }
161 }
162 
163 void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t minSolCount, std::size_t maxSolCount,
165 {
166  OMPL_DEBUG("ParallelPlan.solveMore: starting planner %s", planner->getName().c_str());
167 
168  time::point start = time::now();
169  if (planner->solve(*ptc))
170  {
171  double duration = time::seconds(time::now() - start);
172  foundSolCountLock_.lock();
173  unsigned int nrSol = ++foundSolCount_;
174  foundSolCountLock_.unlock();
175 
176  if (nrSol >= maxSolCount)
177  ptc->terminate();
178 
179  OMPL_DEBUG("ParallelPlan.solveMore: Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
180 
181  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
182 
183  std::lock_guard<std::mutex> slock(phlock_);
184  start = time::now();
185  unsigned int attempts = 0;
186  for (const auto &path : paths)
187  attempts += phybrid_->recordPath(path.path_, false);
188 
189  if (phybrid_->pathCount() >= minSolCount)
190  phybrid_->computeHybridPath();
191 
192  duration = time::seconds(time::now() - start);
193  OMPL_DEBUG("ParallelPlan.solveMore: Spent %f seconds hybridizing %u solution paths (attempted %u connections "
194  "between paths)",
195  duration, (unsigned int)phybrid_->pathCount(), attempts);
196  }
197 }
void clearHybridizationPaths()
Clear the set of paths recorded for hybrididzation.
A shared pointer wrapper for ompl::base::ProblemDefinition.
void addPlannerAllocator(const base::PlannerAllocator &pa)
Add a planner allocator to use.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
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...
base::ProblemDefinitionPtr pdef_
The problem definition used.
Definition: ParallelPlan.h:141
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:76
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...
std::vector< base::PlannerPtr > planners_
The set of planners to be used.
Definition: ParallelPlan.h:144
geometric::PathHybridizationPtr phybrid_
The instance of the class that performs path hybridization.
Definition: ParallelPlan.h:147
A shared pointer wrapper for ompl::base::Planner.
std::mutex phlock_
Lock for phybrid_.
Definition: ParallelPlan.h:150
ParallelPlan(const base::ProblemDefinitionPtr &pdef)
Create an instance for a specified space information.
Base class for a planner.
Definition: Planner.h:223
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:46
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
point now()
Get the current time point.
Definition: Time.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:60
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:433
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns...
A shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68