AnytimePathShortening.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, Rice University
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 Rice University 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: Ryan Luna */
36 
37 #include "ompl/geometric/planners/AnytimePathShortening.h"
38 #include "ompl/geometric/PathHybridization.h"
39 #include "ompl/geometric/PathSimplifier.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
42 
43 #include <boost/thread.hpp>
44 
46  ompl::base::Planner(si, "APS"),
47  shortcut_(true),
48  hybridize_(true),
49  maxHybridPaths_(24),
50  defaultNumPlanners_(std::max(1u, boost::thread::hardware_concurrency()))
51 {
53  specs_.multithreaded = true;
54  specs_.optimizingPaths = true;
55 
56  Planner::declareParam<bool>("shortcut", this, &AnytimePathShortening::setShortcut, &AnytimePathShortening::isShortcutting, "0,1");
57  Planner::declareParam<bool>("hybridize", this, &AnytimePathShortening::setHybridize, &AnytimePathShortening::isHybridizing, "0,1");
58  Planner::declareParam<unsigned int>("max_hybrid_paths", this, &AnytimePathShortening::setMaxHybridizationPath, &AnytimePathShortening::maxHybridizationPaths, "0:1:50");
59  Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners, &AnytimePathShortening::getDefaultNumPlanners, "0:64");
60 
61  addPlannerProgressProperty("best cost REAL",
62  boost::bind(&AnytimePathShortening::getBestCost, this));
63 }
64 
66 {
67 }
68 
70 {
71  if (planner && planner->getSpaceInformation().get() != si_.get())
72  {
73  OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
74  return;
75  }
76 
77  // Ensure all planners are unique instances
78  for(size_t i = 0; i < planners_.size(); ++i)
79  {
80  if (planner.get() == planners_[i].get())
81  {
82  OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
83  return;
84  }
85  }
86 
87  planners_.push_back(planner);
88 }
89 
91 {
93  for (size_t i = 0; i < planners_.size(); ++i)
94  planners_[i]->setProblemDefinition(pdef);
95 }
96 
98 {
99  base::Goal *goal = pdef_->getGoal().get();
100  std::vector<boost::thread*> threads(planners_.size());
101  geometric::PathHybridization phybrid(si_);
102  base::Path *bestSln = NULL;
103 
104  base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
105  if (!opt)
106  {
107  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
108  opt.reset(new base::PathLengthOptimizationObjective(si_));
109  pdef_->setOptimizationObjective(opt);
110  }
111  else
112  {
113  if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt.get()))
114  OMPL_WARN("The optimization objective is not set for path length. The specified optimization criteria may not be optimized over.");
115  }
116 
117  // Disable output from the motion planners, except for errors
118  msg::LogLevel currentLogLevel = msg::getLogLevel();
119  msg::setLogLevel(std::max(msg::LOG_ERROR, currentLogLevel));
120  while (!ptc())
121  {
122  // We have found a solution that is good enough
123  if (bestSln && opt->isSatisfied(base::Cost(bestSln->length())))
124  break;
125 
126  // Clear any previous planning data for the set of planners
127  clear();
128 
129  // Spawn a thread for each planner. This will shortcut the best path after solving.
130  for (size_t i = 0; i < threads.size(); ++i)
131  threads[i] = new boost::thread(boost::bind(&AnytimePathShortening::threadSolve, this, planners_[i].get(), ptc));
132 
133  // Join each thread, and then delete it
134  for (std::size_t i = 0 ; i < threads.size() ; ++i)
135  {
136  threads[i]->join();
137  delete threads[i];
138  }
139 
140  // Hybridize the set of paths computed. Add the new hybrid path to the mix.
141  if (hybridize_ && !ptc())
142  {
143  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
144  for (size_t j = 0; j < paths.size() && j < maxHybridPaths_ && !ptc(); ++j)
145  phybrid.recordPath(paths[j].path_, false);
146 
147  phybrid.computeHybridPath();
148  const base::PathPtr &hsol = phybrid.getHybridPath();
149  if (hsol)
150  {
151  geometric::PathGeometric *pg = static_cast<geometric::PathGeometric*>(hsol.get());
152  double difference = 0.0;
153  bool approximate = !goal->isSatisfied(pg->getStates().back(), &difference);
154  pdef_->addSolutionPath(hsol, approximate, difference);
155  }
156  }
157 
158  // keep track of the best solution found so far
159  if (pdef_->getSolutionCount() > 0)
160  bestSln = pdef_->getSolutionPath().get();
161  }
162  msg::setLogLevel(currentLogLevel);
163 
164  if (bestSln)
165  {
166  if (goal->isSatisfied (static_cast<geometric::PathGeometric*>(bestSln)->getStates().back()))
169  }
171 }
172 
174 {
175  // compute a motion plan
176  base::PlannerStatus status = planner->solve(ptc);
177 
178  // Shortcut the best solution found so far
179  if (shortcut_ && status == base::PlannerStatus::EXACT_SOLUTION)
180  {
181  geometric::PathGeometric* sln = static_cast<geometric::PathGeometric*>(pdef_->getSolutionPath().get());
183  geometric::PathSimplifier ps(pdef_->getSpaceInformation());
184  if (ps.shortcutPath(*pathCopy))
185  {
186  double difference = 0.0;
187  bool approximate = !pdef_->getGoal()->isSatisfied(pathCopy->getStates().back(), &difference);
188  pdef_->addSolutionPath(base::PathPtr(pathCopy), approximate, difference);
189  }
190  else delete pathCopy;
191  }
192 }
193 
195 {
196  Planner::clear();
197  for (size_t i = 0; i < planners_.size(); ++i)
198  planners_[i]->clear();
199 }
200 
202 {
203  if (planners_.size() == 0)
204  return;
205 
206  OMPL_WARN("Returning planner data for planner #0");
207  getPlannerData(data, 0);
208 }
209 
211 {
212  if(planners_.size() < idx)
213  return;
214  planners_[idx]->getPlannerData(data);
215 }
216 
218 {
219  Planner::setup();
220 
221  if (planners_.size() == 0)
222  {
223  planners_.reserve(defaultNumPlanners_);
224  for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
225  {
226  planners_.push_back(tools::SelfConfig::getDefaultPlanner(pdef_->getGoal()));
227  planners_.back()->setProblemDefinition(pdef_);
228  }
229  OMPL_INFORM("%s: No planners specified; using %u instances of %s",
230  getName().c_str(), planners_.size(), planners_[0]->getName().c_str());
231  }
232 
233  for (size_t i = 0; i < planners_.size(); ++i)
234  planners_[i]->setup();
235 }
236 
238 {
239  for (size_t i = 0; i < planners_.size(); ++i)
240  planners_[i]->checkValidity();
241 }
242 
244 {
245  return planners_.size();
246 }
247 
249 {
250  assert(idx < planners_.size());
251  return planners_[idx];
252 }
253 
255 {
256  return shortcut_;
257 }
258 
260 {
261  shortcut_ = shortcut;
262 }
263 
265 {
266  return hybridize_;
267 }
268 
270 {
271  hybridize_ = hybridize;
272 }
273 
275 {
276  return maxHybridPaths_;
277 }
278 
280 {
281  maxHybridPaths_ = maxPathCount;
282 }
283 
285 {
286  defaultNumPlanners_ = numPlanners;
287 }
288 
290 {
291  return defaultNumPlanners_;
292 }
293 
295 {
296  base::Cost bestCost(std::numeric_limits<double>::quiet_NaN());
297  if (pdef_ && pdef_->getSolutionCount() > 0)
298  bestCost = base::Cost(pdef_->getSolutionPath()->length());
299  return boost::lexical_cast<std::string>(bestCost);
300 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:391
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
std::string getBestCost() const
Return best cost found so far by algorithm.
bool isShortcutting(void) const
Return whether the anytime planner will perform shortcutting on paths.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the most recent run of the motion planner.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Method that solves the motion planning problem. This method terminates under just two conditions...
bool isHybridizing(void) const
Return whether the anytime planner will extract a hybrid path from the set of solution paths...
void setMaxHybridizationPath(unsigned int maxPathCount)
Set the maximum number of paths that will be hybridized.
Abstract definition of goals.
Definition: Goal.h:63
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
AnytimePathShortening(const base::SpaceInformationPtr &si)
Constructor requires the space information to plan in.
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:211
void setHybridize(bool hybridize)
Enable/disable path hybridization on the set of solution paths.
void setDefaultNumPlanners(unsigned int numPlanners)
Set default number of planners to use if none are specified.
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planners. The problem needs to be set before calling solve()...
Main namespace. Contains everything in this library.
Definition: Cost.h:42
A boost shared pointer wrapper for ompl::base::Planner.
virtual void clear(void)
Clear all internal planning datastructures. Planner settings are not affected. Subsequent calls to so...
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...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Base class for a planner.
Definition: Planner.h:232
LogLevel getLogLevel()
Retrieve the current level of logging data. Messages with lower logging levels will not be recorded...
Definition: Console.cpp:126
The planner found an exact solution.
Definition: PlannerStatus.h:66
void setLogLevel(LogLevel level)
Set the minimum level of logging data to output. Messages with lower logging levels will not be recor...
Definition: Console.cpp:120
virtual void threadSolve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
The function that the planning threads execute when solving a motion planning problem.
void setShortcut(bool shortcut)
Enable/disable shortcutting on paths.
Abstract definition of a path.
Definition: Path.h:67
virtual void setup(void)
Perform any necessary configuration steps. This method also invokes ompl::base::SpaceInformation::set...
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
unsigned int maxHybridizationPaths(void) const
Return the maximum number of paths that will be hybridized.
virtual double length() const =0
Return the length of a path.
This class contains routines that attempt to simplify geometric paths.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: Planner.cpp:75
An optimization objective which corresponds to optimizing path length.
virtual void checkValidity(void)
Check to see if the planners are in a working state (setup has been called, a goal was set...
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
unsigned int getNumPlanners(void) const
Retrieve the number of planners added.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:250
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 planner found an approximate solution.
Definition: PlannerStatus.h:64
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
void addPlanner(base::PlannerPtr &planner)
Adds the given planner to the set of planners used to compute candidate paths.
virtual ~AnytimePathShortening(void)
Destructor.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:218
Definition of a geometric path.
Definition: PathGeometric.h:60
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution...
LogLevel
The set of priorities for message logging.
Definition: Console.h:81
base::PlannerPtr getPlanner(unsigned int i) const
Retrieve a pointer to the ith planner instance.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
A boost shared pointer wrapper for ompl::base::Path.
unsigned int getDefaultNumPlanners() const
Get default number of planners used if none are specified.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68