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)
95 }
96 
98 {
99  base::Goal *goal = pdef_->getGoal().get();
100  std::vector<boost::thread*> threads(planners_.size());
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());
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
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  {
224  for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
225  {
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
unsigned int maxHybridizationPaths(void) const
Return the maximum number of paths that will be hybridized.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
base::PlannerPtr getPlanner(unsigned int i) const
Retrieve a pointer to the ith planner instance.
unsigned int getNumPlanners(void) const
Retrieve the number of planners added.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Method that solves the motion planning problem. This method terminates under just two conditions...
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.
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:211
unsigned int getDefaultNumPlanners() const
Get default number of planners used if none are specified.
bool isHybridizing(void) const
Return whether the anytime planner will extract a hybrid path from the set of solution paths...
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.
std::string getBestCost() const
Return best cost found so far by algorithm.
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
unsigned int maxHybridPaths_
The maximum number of paths that will be hybridized. This prohibits hybridization of a very large pat...
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
bool hybridize_
Flag indicating whether to hybridize the set of solution paths.
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
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...
unsigned int defaultNumPlanners_
The number of planners to use if none are specified. This defaults to the number of cores...
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
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.
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
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
std::vector< base::PlannerPtr > planners_
The list of planners used for solving the problem.
Definition of a geometric path.
Definition: PathGeometric.h:60
virtual void getPlannerData(base::PlannerData &data) const
Get information about the most recent run of the motion planner.
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
bool isShortcutting(void) const
Return whether the anytime planner will perform shortcutting on paths.
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
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.
bool shortcut_
Flag indicating whether to shortcut paths.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68