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 <algorithm> // for std::min
38 #include <boost/algorithm/string.hpp>
39 
40 #include "ompl/geometric/planners/AnytimePathShortening.h"
41 #include "ompl/geometric/planners/fmt/BFMT.h"
42 #include "ompl/geometric/planners/est/BiEST.h"
43 #include "ompl/geometric/planners/rrt/BiTRRT.h"
44 #include "ompl/geometric/planners/informedtrees/BITstar.h"
45 #include "ompl/geometric/planners/kpiece/BKPIECE1.h"
46 #include "ompl/geometric/planners/est/EST.h"
47 #include "ompl/geometric/planners/fmt/FMT.h"
48 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
49 #include "ompl/geometric/planners/rrt/LazyLBTRRT.h"
50 #include "ompl/geometric/planners/prm/LazyPRM.h"
51 #include "ompl/geometric/planners/prm/LazyPRMstar.h"
52 #include "ompl/geometric/planners/rrt/LazyRRT.h"
53 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
54 #include "ompl/geometric/planners/rrt/LBTRRT.h"
55 #include "ompl/geometric/planners/pdst/PDST.h"
56 #include "ompl/geometric/planners/prm/PRM.h"
57 #include "ompl/geometric/planners/prm/PRMstar.h"
58 #include "ompl/geometric/planners/est/ProjEST.h"
59 #include "ompl/geometric/planners/rrt/RRT.h"
60 #include "ompl/geometric/planners/rrt/RRTConnect.h"
61 #include "ompl/geometric/planners/rrt/RRTstar.h"
62 #include "ompl/geometric/planners/rrt/RRTXstatic.h"
63 #include "ompl/geometric/planners/sbl/SBL.h"
64 #include "ompl/geometric/planners/prm/SPARS.h"
65 #include "ompl/geometric/planners/prm/SPARStwo.h"
66 #include "ompl/geometric/planners/sst/SST.h"
67 #include "ompl/geometric/planners/stride/STRIDE.h"
68 #include "ompl/geometric/planners/rrt/TRRT.h"
69 #include "ompl/geometric/PathHybridization.h"
70 #include "ompl/geometric/PathSimplifier.h"
71 #include "ompl/tools/config/SelfConfig.h"
72 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
73 #include "ompl/util/String.h"
74 
76  : ompl::base::Planner(si, "APS"), defaultNumPlanners_(std::max(1u, std::thread::hardware_concurrency()))
77 {
79  specs_.multithreaded = true;
80  specs_.optimizingPaths = true;
81 
82  Planner::declareParam<bool>("shortcut", this, &AnytimePathShortening::setShortcut,
84  Planner::declareParam<bool>("hybridize", this, &AnytimePathShortening::setHybridize,
86  Planner::declareParam<unsigned int>("max_hybrid_paths", this, &AnytimePathShortening::setMaxHybridizationPath,
88  Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners,
90  Planner::declareParam<std::string>("planners", this, &AnytimePathShortening::setPlanners,
92 
93  addPlannerProgressProperty("best cost REAL", [this] { return getBestCost(); });
94 }
95 
97 
99 {
100  if (planner && planner->getSpaceInformation().get() != si_.get())
101  {
102  OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
103  return;
104  }
105 
106  // Ensure all planners are unique instances
107  for (auto &i : planners_)
108  {
109  if (planner.get() == i.get())
110  {
111  OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
112  return;
113  }
114  }
115 
116  planners_.push_back(planner);
117 }
118 
119 void ompl::geometric::AnytimePathShortening::addPath(const geometric::PathGeometricPtr &path, base::Planner *planner)
120 {
121  const base::OptimizationObjectivePtr &opt(pdef_->getOptimizationObjective());
122  base::Cost pathCost = path->cost(opt);
123  std::lock_guard<std::mutex> _(lock_);
124  if (opt->isCostBetterThan(pathCost, bestCost_))
125  {
126  bestCost_ = pathCost;
127  pdef_->addSolutionPath(path, false, 0.0, planner->getName());
128  }
129  else if (planner != this)
130  pdef_->addSolutionPath(path, false, 0.0, planner->getName());
131 }
132 
135 {
136  std::vector<std::thread> threads;
137  geometric::PathHybridization phybrid(si_);
138 
139  base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
140  if (!opt)
141  {
142  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
143  "planning time.",
144  getName().c_str());
145  opt = std::make_shared<base::PathLengthOptimizationObjective>(si_);
146  pdef_->setOptimizationObjective(opt);
147  }
148 
149  // Disable output from the motion planners, except for errors
150  msg::LogLevel currentLogLevel = msg::getLogLevel();
151  msg::setLogLevel(std::max(msg::LOG_ERROR, currentLogLevel));
152 
153  // Clear any previous planning data for the set of planners
154  clear();
155  // Spawn a thread for each planner. This will shortcut the best path after solving.
156  for (auto &planner : planners_)
157  threads.emplace_back([this, planner, &ptc] { return threadSolve(planner.get(), ptc); });
158 
160  geometric::PathGeometric *sln = nullptr, *prevLastPath = nullptr;
161  bestCost_ = opt->infiniteCost();
162  std::size_t prevSolCount = 0;
163  while (!ptc)
164  {
165  // We have found a solution that is good enough
166  if (opt->isSatisfied(bestCost_))
167  {
168  ptc.terminate();
169  break;
170  }
171 
172  // Hybridize the set of paths computed. Add the new hybrid path to the mix.
173  unsigned int solCount = pdef_->getSolutionCount();
174  if (hybridize_ && !ptc && solCount > 1)
175  {
176  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
177  std::size_t numPaths = std::min(solCount, maxHybridPaths_);
178  geometric::PathGeometric *lastPath = static_cast<PathGeometric *>(paths[numPaths - 1].path_.get());
179  // check if new solution paths have been added to top numPaths paths
180  if (lastPath != prevLastPath || (prevSolCount < solCount && solCount <= maxHybridPaths_))
181  {
182  for (size_t j = 0; j < numPaths && !ptc; ++j)
183  phybrid.recordPath(std::static_pointer_cast<PathGeometric>(paths[j].path_), false);
184 
185  phybrid.computeHybridPath();
186  sln = phybrid.getHybridPath().get();
187  prevLastPath = lastPath;
188  }
189  else
190  sln = static_cast<PathGeometric *>(pdef_->getSolutionPath().get());
191  prevSolCount = solCount;
192  }
193  else if (solCount > 0)
194  sln = static_cast<PathGeometric *>(pdef_->getSolutionPath().get());
195 
196  if (sln)
197  {
198  auto pathCopy(std::make_shared<geometric::PathGeometric>(*sln));
199  if (shortcut_) // Shortcut the path
200  if (!ps.simplify(*pathCopy, ptc, true))
201  // revert if shortcutting failed
202  pathCopy = std::make_shared<geometric::PathGeometric>(*sln);
203  addPath(pathCopy, this);
204  }
205  if (hybridize_ && phybrid.pathCount() >= maxHybridPaths_)
206  phybrid.clear();
207  }
208 
209  for (auto &thread : threads)
210  thread.join();
211 
212  msg::setLogLevel(currentLogLevel);
213  return pdef_->getSolutionCount() > 0 ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::UNKNOWN;
214 }
215 
218 {
219  // create a local clone of the problem definition where we keep at most one solution
220  auto pdef = pdef_->clone();
222 
223  planner->setProblemDefinition(pdef);
224  while (!ptc)
225  {
226  if (planner->solve(ptc) == base::PlannerStatus::EXACT_SOLUTION)
227  {
228  geometric::PathGeometric *sln = static_cast<geometric::PathGeometric *>(pdef->getSolutionPath().get());
229  auto pathCopy(std::make_shared<geometric::PathGeometric>(*sln));
230  if (shortcut_) // Shortcut the path
231  ps.shortcutPath(*pathCopy);
232  addPath(pathCopy, planner);
233  }
234 
235  planner->clear();
236  pdef->clearSolutionPaths();
237  }
238 }
239 
241 {
242  Planner::clear();
243  for (auto &planner : planners_)
244  planner->clear();
245  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
246 }
247 
249 {
250  if (planners_.empty())
251  return;
252 
253  OMPL_WARN("Returning planner data for planner #0");
254  getPlannerData(data, 0);
255 }
256 
258 {
259  if (planners_.size() < idx)
260  return;
261  planners_[idx]->getPlannerData(data);
262 }
263 
265 {
266  Planner::setup();
267 
268  if (planners_.empty())
269  {
270  planners_.reserve(defaultNumPlanners_);
271  for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
272  {
273  planners_.push_back(tools::SelfConfig::getDefaultPlanner(pdef_->getGoal()));
274  planners_.back()->setProblemDefinition(pdef_);
275  }
276  OMPL_INFORM("%s: No planners specified; using %u instances of %s", getName().c_str(), planners_.size(),
277  planners_[0]->getName().c_str());
278  }
279 
280  for (auto &planner : planners_)
281  planner->setup();
282 }
283 
285 {
286  for (auto &planner : planners_)
287  planner->checkValidity();
288 }
289 
291 {
292  return planners_.size();
293 }
294 
296 {
297  assert(idx < planners_.size());
298  return planners_[idx];
299 }
300 
302 {
303  return shortcut_;
304 }
305 
307 {
308  shortcut_ = shortcut;
309 }
310 
312 {
313  return hybridize_;
314 }
315 
317 {
318  hybridize_ = hybridize;
319 }
320 
322 {
323  return maxHybridPaths_;
324 }
325 
327 {
328  maxHybridPaths_ = maxPathCount;
329 }
330 
332 {
333  defaultNumPlanners_ = numPlanners;
334 }
335 
337 {
338  return defaultNumPlanners_;
339 }
340 
342 {
343  return ompl::toString(bestCost_.value());
344 }
345 
346 void ompl::geometric::AnytimePathShortening::setPlanners(const std::string &plannerList)
347 {
348  std::vector<std::string> plannerStrings;
349 
350  boost::split(plannerStrings, plannerList, boost::is_any_of(","));
351 
352  planners_.clear();
353  for (const auto &plannerString : plannerStrings)
354  {
355  std::vector<std::string> plannerAndParams;
356  boost::split(plannerAndParams, plannerString, boost::is_any_of("[ ]"));
357  const std::string &plannerName = plannerAndParams[0];
358 
359  if (plannerName == "BFMT")
360  planners_.push_back(std::make_shared<geometric::BFMT>(si_));
361  else if (plannerName == "BiEST")
362  planners_.push_back(std::make_shared<geometric::BiEST>(si_));
363  else if (plannerName == "BiTRRT")
364  planners_.push_back(std::make_shared<geometric::BiTRRT>(si_));
365  else if (plannerName == "BITstar")
366  planners_.push_back(std::make_shared<geometric::BITstar>(si_));
367  else if (plannerName == "BKPIECE")
368  planners_.push_back(std::make_shared<geometric::BKPIECE1>(si_));
369  else if (plannerName == "EST")
370  planners_.push_back(std::make_shared<geometric::EST>(si_));
371  else if (plannerName == "FMT")
372  planners_.push_back(std::make_shared<geometric::FMT>(si_));
373  else if (plannerName == "KPIECE")
374  planners_.push_back(std::make_shared<geometric::KPIECE1>(si_));
375  else if (plannerName == "LazyLBTRRT")
376  planners_.push_back(std::make_shared<geometric::LazyLBTRRT>(si_));
377  else if (plannerName == "LazyPRM")
378  planners_.push_back(std::make_shared<geometric::LazyPRM>(si_));
379  else if (plannerName == "LazyPRMstar")
380  planners_.push_back(std::make_shared<geometric::LazyPRMstar>(si_));
381  else if (plannerName == "LazyRRT")
382  planners_.push_back(std::make_shared<geometric::LazyRRT>(si_));
383  else if (plannerName == "LBKPIECE")
384  planners_.push_back(std::make_shared<geometric::LBKPIECE1>(si_));
385  else if (plannerName == "LBTRRT")
386  planners_.push_back(std::make_shared<geometric::LBTRRT>(si_));
387  else if (plannerName == "PDST")
388  planners_.push_back(std::make_shared<geometric::PDST>(si_));
389  else if (plannerName == "PRM")
390  planners_.push_back(std::make_shared<geometric::PRM>(si_));
391  else if (plannerName == "PRMstar")
392  planners_.push_back(std::make_shared<geometric::PRMstar>(si_));
393  else if (plannerName == "ProjEST")
394  planners_.push_back(std::make_shared<geometric::ProjEST>(si_));
395  else if (plannerName == "RRT")
396  planners_.push_back(std::make_shared<geometric::RRT>(si_));
397  else if (plannerName == "RRTConnect")
398  planners_.push_back(std::make_shared<geometric::RRTConnect>(si_));
399  else if (plannerName == "RRTstar")
400  planners_.push_back(std::make_shared<geometric::RRTstar>(si_));
401  else if (plannerName == "RRTXstatic")
402  planners_.push_back(std::make_shared<geometric::RRTXstatic>(si_));
403  else if (plannerName == "SBL")
404  planners_.push_back(std::make_shared<geometric::SBL>(si_));
405  else if (plannerName == "SPARS")
406  planners_.push_back(std::make_shared<geometric::SPARS>(si_));
407  else if (plannerName == "SPARStwo")
408  planners_.push_back(std::make_shared<geometric::SPARStwo>(si_));
409  else if (plannerName == "SST")
410  planners_.push_back(std::make_shared<geometric::SST>(si_));
411  else if (plannerName == "STRIDE")
412  planners_.push_back(std::make_shared<geometric::STRIDE>(si_));
413  else if (plannerName == "TRRT")
414  planners_.push_back(std::make_shared<geometric::TRRT>(si_));
415  else
416  OMPL_ERROR("Unknown planner name: %s", plannerName.c_str());
417 
418  std::vector<std::string> paramValue;
419  for (auto it = plannerAndParams.begin() + 1; it != plannerAndParams.end(); ++it)
420  if (!it->empty())
421  {
422  boost::split(paramValue, *it, boost::is_any_of("="));
423  planners_.back()->params().setParam(paramValue[0], paramValue[1]);
424  }
425  }
426 }
427 
429 {
430  std::stringstream ss;
431  for (unsigned int i = 0; i < planners_.size(); ++i)
432  {
433  if (i > 0)
434  ss << ',';
435  ss << planners_[i]->getName();
436 
437  std::map<std::string, std::string> params;
438  planners_[i]->params().getParams(params);
439  if (params.size() > 0)
440  {
441  ss << '[';
442  for (auto it = params.begin(); it != params.end(); ++it)
443  {
444  if (it != params.begin())
445  ss << ' ';
446  ss << it->first << '=' << it->second;
447  }
448  ss << ']';
449  }
450  }
451  return ss.str();
452 }
453 
455 {
456  Planner::printSettings(out);
457  out << "Settings for planner instances in AnytimePathShortening instance:\n";
458  for (const auto &planner : planners_)
459  {
460  out << "* ";
461  planner->printSettings(out);
462  }
463 }
Base class for a planner.
Definition: Planner.h:279
unsigned int maxHybridizationPaths() const
Return the maximum number of paths that will be hybridized.
std::string getPlanners() const
Get a string representation of the planners and their parameters in the format of setPlanners.
A shared pointer wrapper for ompl::base::SpaceInformation.
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:256
unsigned int recordPath(const geometric::PathGeometricPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
void getPlannerData(base::PlannerData &data) const override
Get information about the most recent run of the motion planner.
void clear()
Clear all the stored paths.
bool isShortcutting() const
Return whether the anytime planner will perform shortcutting on paths.
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:467
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
This class contains routines that attempt to simplify geometric paths.
const geometric::PathGeometricPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce=true)
Run simplification algorithms on the path for at most maxTime seconds, and at least once if atLeastOn...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
base::PlannerPtr getPlanner(unsigned int idx) const
Retrieve a pointer to the ith planner instance.
unsigned int getNumPlanners() const
Retrieve the number of planners added.
Definition of a geometric path.
Definition: PathGeometric.h:97
A shared pointer wrapper for ompl::base::Planner.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
void setup() override
Perform any necessary configuration steps. This method also invokes ompl::base::SpaceInformation::set...
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void computeHybridPath()
Run Dijkstra's algorithm to find out the lowest-cost path among the mixed ones.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
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:136
LogLevel
The set of priorities for message logging.
Definition: Console.h:84
AnytimePathShortening(const base::SpaceInformationPtr &si)
Constructor requires the space information to plan in.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:263
bool isHybridizing() const
Return whether the anytime planner will extract a hybrid path from the set of solution paths.
virtual void threadSolve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
The function that the planning threads execute when solving a motion planning problem.
A class to store the exit status of Planner::solve()
void setDefaultNumPlanners(unsigned int numPlanners)
Set default number of planners to use if none are specified.
void addPath(const geometric::PathGeometricPtr &path, base::Planner *planner)
add a path to set of solutions
void printSettings(std::ostream &out) const override
Print settings of this planner as well as those of the planner instances it contains.
void setMaxHybridizationPath(unsigned int maxPathCount)
Set the maximum number of paths that will be hybridized.
void setHybridize(bool hybridize)
Enable/disable path hybridization on the set of solution paths.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
unsigned int getDefaultNumPlanners() const
Get default number of planners used if none are specified.
bool shortcutPath(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33, double snapToVertex=0.005)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
@ EXACT_SOLUTION
The planner found an exact solution.
@ UNKNOWN
Uninitialized status.
std::string getBestCost() const
Return best cost found so far by algorithm.
void clear() override
Clear all internal planning datastructures. Planner settings are not affected. Subsequent calls to so...
LogLevel getLogLevel()
Retrieve the current level of logging data. Messages with lower logging levels will not be recorded.
Definition: Console.cpp:142
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void setPlanners(const std::string &plannerList)
Set the list of planners to use.
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
~AnytimePathShortening() override
Destructor.
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:243
void addPlanner(base::PlannerPtr &planner)
Adds the given planner to the set of planners used to compute candidate paths.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Method that solves the motion planning problem. This method terminates under just two conditions,...
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setShortcut(bool shortcut)
Enable/disable shortcutting on paths.
void checkValidity() override
Check to see if the planners are in a working state (setup has been called, a goal was set,...