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
41ompl::tools::ParallelPlan::ParallelPlan(const base::ProblemDefinitionPtr &pdef)
42 : pdef_(pdef), phybrid_(std::make_shared<geometric::PathHybridization>(pdef->getSpaceInformation()))
43{
44}
45
46ompl::tools::ParallelPlan::~ParallelPlan() = default;
47
48void 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
79ompl::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
144void 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 if (planner->solve(*ptc))
151 {
152 double duration = time::seconds(time::now() - start);
153 foundSolCountLock_.lock();
154 unsigned int nrSol = ++foundSolCount_;
155 foundSolCountLock_.unlock();
156 if (nrSol >= minSolCount)
157 ptc->terminate();
158 OMPL_DEBUG("ParallelPlan.solveOne: Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
159 }
160}
161
162void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t minSolCount, std::size_t maxSolCount,
164{
165 OMPL_DEBUG("ParallelPlan.solveMore: starting planner %s", planner->getName().c_str());
166
167 time::point start = time::now();
168 if (planner->solve(*ptc))
169 {
170 double duration = time::seconds(time::now() - start);
171 foundSolCountLock_.lock();
172 unsigned int nrSol = ++foundSolCount_;
173 foundSolCountLock_.unlock();
174
175 if (nrSol >= maxSolCount)
176 ptc->terminate();
177
178 OMPL_DEBUG("ParallelPlan.solveMore: Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
179
180 const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
181
182 std::lock_guard<std::mutex> slock(phlock_);
183 start = time::now();
184 unsigned int attempts = 0;
185 for (const auto &path : paths)
186 attempts += phybrid_->recordPath(std::static_pointer_cast<geometric::PathGeometric>(path.path_), false);
187
188 if (phybrid_->pathCount() >= minSolCount)
189 phybrid_->computeHybridPath();
190
191 duration = time::seconds(time::now() - start);
192 OMPL_DEBUG("ParallelPlan.solveMore: Spent %f seconds hybridizing %u solution paths (attempted %u connections "
193 "between paths)",
194 duration, (unsigned int)phybrid_->pathCount(), attempts);
195 }
196}
The exception type for ompl.
Definition: Exception.h:47
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
Base class for a planner.
Definition: Planner.h:223
void addPlanner(const base::PlannerPtr &planner)
Add a planner to use.
void addPlannerAllocator(const base::PlannerAllocator &pa)
Add a planner allocator to use.
ParallelPlan(const base::ProblemDefinitionPtr &pdef)
Create an instance for a specified space information.
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 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 ...
void clearPlanners()
Clear the set of planners to be executed.
base::PlannerStatus solve(double solveTime, bool hybridize=true)
Call Planner::solve() for all planners, in parallel, each planner running for at most solveTime secon...
void clearHybridizationPaths()
Clear the set of paths recorded for hybrididzation.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:444
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
point now()
Get the current time point.
Definition: Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
STL namespace.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49