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  logInform("Solution found in %f seconds", time::seconds(time::now() - start));
125 
126  return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
127 }
128 
130 {
131  logDebug("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  logDebug("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  logDebug("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  logDebug("Spent %f seconds hybridizing %u solution paths (attempted %u connections between paths)", duration, (unsigned int)phybrid_->pathCount(), attempts);
173  }
174 }