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 <boost/algorithm/string.hpp>
38 
39 #include "ompl/geometric/planners/AnytimePathShortening.h"
40 #include "ompl/geometric/planners/fmt/BFMT.h"
41 #include "ompl/geometric/planners/est/BiEST.h"
42 #include "ompl/geometric/planners/rrt/BiTRRT.h"
43 #include "ompl/geometric/planners/bitstar/BITstar.h"
44 #include "ompl/geometric/planners/kpiece/BKPIECE1.h"
45 #include "ompl/geometric/planners/est/EST.h"
46 #include "ompl/geometric/planners/fmt/FMT.h"
47 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
48 #include "ompl/geometric/planners/rrt/LazyLBTRRT.h"
49 #include "ompl/geometric/planners/prm/LazyPRM.h"
50 #include "ompl/geometric/planners/prm/LazyPRMstar.h"
51 #include "ompl/geometric/planners/rrt/LazyRRT.h"
52 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
53 #include "ompl/geometric/planners/rrt/LBTRRT.h"
54 #include "ompl/geometric/planners/pdst/PDST.h"
55 #include "ompl/geometric/planners/prm/PRM.h"
56 #include "ompl/geometric/planners/prm/PRMstar.h"
57 #include "ompl/geometric/planners/est/ProjEST.h"
58 #include "ompl/geometric/planners/rrt/RRT.h"
59 #include "ompl/geometric/planners/rrt/RRTConnect.h"
60 #include "ompl/geometric/planners/rrt/RRTstar.h"
61 #include "ompl/geometric/planners/rrt/RRTXstatic.h"
62 #include "ompl/geometric/planners/sbl/SBL.h"
63 #include "ompl/geometric/planners/prm/SPARS.h"
64 #include "ompl/geometric/planners/prm/SPARStwo.h"
65 #include "ompl/geometric/planners/sst/SST.h"
66 #include "ompl/geometric/planners/stride/STRIDE.h"
67 #include "ompl/geometric/planners/rrt/TRRT.h"
68 #include "ompl/geometric/PathHybridization.h"
69 #include "ompl/geometric/PathSimplifier.h"
70 #include "ompl/tools/config/SelfConfig.h"
71 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
72 #include "ompl/util/String.h"
73 
75  : ompl::base::Planner(si, "APS")
76  , 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]
94  {
95  return getBestCost();
96  });
97 }
98 
100 
101 void ompl::geometric::AnytimePathShortening::addPlanner(base::PlannerPtr &planner)
102 {
103  if (planner && planner->getSpaceInformation().get() != si_.get())
104  {
105  OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
106  return;
107  }
108 
109  // Ensure all planners are unique instances
110  for (auto &i : planners_)
111  {
112  if (planner.get() == i.get())
113  {
114  OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
115  return;
116  }
117  }
118 
119  planners_.push_back(planner);
120 }
121 
123 {
125  for (auto &planner : planners_)
126  planner->setProblemDefinition(pdef);
127 }
128 
131 {
132  base::Goal *goal = pdef_->getGoal().get();
133  std::vector<std::thread *> threads(planners_.size());
134  geometric::PathHybridization phybrid(si_);
135  base::Path *bestSln = nullptr;
136 
137  base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
138  if (!opt)
139  {
140  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
141  "planning time.",
142  getName().c_str());
143  opt = std::make_shared<base::PathLengthOptimizationObjective>(si_);
144  pdef_->setOptimizationObjective(opt);
145  }
146  else
147  {
148  if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt.get()) == nullptr)
149  OMPL_WARN("The optimization objective is not set for path length. The specified optimization criteria may "
150  "not be optimized over.");
151  }
152 
153  // Disable output from the motion planners, except for errors
154  msg::LogLevel currentLogLevel = msg::getLogLevel();
155  msg::setLogLevel(std::max(msg::LOG_ERROR, currentLogLevel));
156  while (!ptc())
157  {
158  // We have found a solution that is good enough
159  if ((bestSln != nullptr) && opt->isSatisfied(base::Cost(bestSln->length())))
160  break;
161 
162  // Clear any previous planning data for the set of planners
163  clear();
164 
165  // Spawn a thread for each planner. This will shortcut the best path after solving.
166  for (size_t i = 0; i < threads.size(); ++i)
167  threads[i] = new std::thread([this, i, &ptc]
168  {
169  return threadSolve(planners_[i].get(), ptc);
170  });
171 
172  // Join each thread, and then delete it
173  for (auto &thread : threads)
174  {
175  thread->join();
176  delete thread;
177  }
178 
179  // Hybridize the set of paths computed. Add the new hybrid path to the mix.
180  if (hybridize_ && !ptc())
181  {
182  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
183  for (size_t j = 0; j < paths.size() && j < maxHybridPaths_ && !ptc(); ++j)
184  phybrid.recordPath(paths[j].path_, false);
185 
186  phybrid.computeHybridPath();
187  const base::PathPtr &hsol = phybrid.getHybridPath();
188  if (hsol)
189  {
190  auto *pg = static_cast<geometric::PathGeometric *>(hsol.get());
191  double difference = 0.0;
192  bool approximate = !goal->isSatisfied(pg->getStates().back(), &difference);
193  pdef_->addSolutionPath(hsol, approximate, difference);
194  }
195  }
196 
197  // keep track of the best solution found so far
198  if (pdef_->getSolutionCount() > 0)
199  bestSln = pdef_->getSolutionPath().get();
200  }
201  msg::setLogLevel(currentLogLevel);
202 
203  if (bestSln != nullptr)
204  {
205  if (goal->isSatisfied(static_cast<geometric::PathGeometric *>(bestSln)->getStates().back()))
208  }
210 }
211 
214 {
215  // compute a motion plan
216  base::PlannerStatus status = planner->solve(ptc);
217 
218  // Shortcut the best solution found so far
219  if (shortcut_ && status == base::PlannerStatus::EXACT_SOLUTION)
220  {
221  geometric::PathGeometric *sln = static_cast<geometric::PathGeometric *>(pdef_->getSolutionPath().get());
222  auto pathCopy(std::make_shared<geometric::PathGeometric>(*sln));
223  geometric::PathSimplifier ps(pdef_->getSpaceInformation());
224  if (ps.shortcutPath(*pathCopy))
225  {
226  double difference = 0.0;
227  bool approximate = !pdef_->getGoal()->isSatisfied(pathCopy->getStates().back(), &difference);
228  pdef_->addSolutionPath(pathCopy, approximate, difference);
229  }
230  }
231 }
232 
234 {
235  Planner::clear();
236  for (auto &planner : planners_)
237  planner->clear();
238 }
239 
241 {
242  if (planners_.empty())
243  return;
244 
245  OMPL_WARN("Returning planner data for planner #0");
246  getPlannerData(data, 0);
247 }
248 
250 {
251  if (planners_.size() < idx)
252  return;
253  planners_[idx]->getPlannerData(data);
254 }
255 
257 {
258  Planner::setup();
259 
260  if (planners_.empty())
261  {
262  planners_.reserve(defaultNumPlanners_);
263  for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
264  {
265  planners_.push_back(tools::SelfConfig::getDefaultPlanner(pdef_->getGoal()));
266  planners_.back()->setProblemDefinition(pdef_);
267  }
268  OMPL_INFORM("%s: No planners specified; using %u instances of %s", getName().c_str(), planners_.size(),
269  planners_[0]->getName().c_str());
270  }
271 
272  for (auto &planner : planners_)
273  planner->setup();
274 }
275 
277 {
278  for (auto &planner : planners_)
279  planner->checkValidity();
280 }
281 
283 {
284  return planners_.size();
285 }
286 
288 {
289  assert(idx < planners_.size());
290  return planners_[idx];
291 }
292 
294 {
295  return shortcut_;
296 }
297 
299 {
300  shortcut_ = shortcut;
301 }
302 
304 {
305  return hybridize_;
306 }
307 
309 {
310  hybridize_ = hybridize;
311 }
312 
314 {
315  return maxHybridPaths_;
316 }
317 
319 {
320  maxHybridPaths_ = maxPathCount;
321 }
322 
324 {
325  defaultNumPlanners_ = numPlanners;
326 }
327 
329 {
330  return defaultNumPlanners_;
331 }
332 
334 {
335  base::Cost bestCost(std::numeric_limits<double>::quiet_NaN());
336  if (pdef_ && pdef_->getSolutionCount() > 0)
337  bestCost = base::Cost(pdef_->getSolutionPath()->length());
338  return ompl::toString(bestCost.value());
339 }
340 
341 void ompl::geometric::AnytimePathShortening::setPlanners(const std::string &plannerList)
342 {
343  std::vector<std::string> plannerStrings;
344 
345  boost::split(plannerStrings, plannerList, boost::is_any_of(","));
346 
347  planners_.clear();
348  for (const auto &plannerString : plannerStrings)
349  {
350  std::vector<std::string> plannerAndParams;
351  boost::split(plannerAndParams, plannerString, boost::is_any_of("[ ]"));
352  const std::string &plannerName = plannerAndParams[0];
353 
354  if (plannerName == "BFMT")
355  planners_.push_back(std::make_shared<geometric::BFMT>(si_));
356  else if (plannerName == "BiEST")
357  planners_.push_back(std::make_shared<geometric::BiEST>(si_));
358  else if (plannerName == "BiTRRT")
359  planners_.push_back(std::make_shared<geometric::BiTRRT>(si_));
360  else if (plannerName == "BITstar")
361  planners_.push_back(std::make_shared<geometric::BITstar>(si_));
362  else if (plannerName == "BKPIECE")
363  planners_.push_back(std::make_shared<geometric::BKPIECE1>(si_));
364  else if (plannerName == "EST")
365  planners_.push_back(std::make_shared<geometric::EST>(si_));
366  else if (plannerName == "FMT")
367  planners_.push_back(std::make_shared<geometric::FMT>(si_));
368  else if (plannerName == "KPIECE")
369  planners_.push_back(std::make_shared<geometric::KPIECE1>(si_));
370  else if (plannerName == "LazyLBTRRT")
371  planners_.push_back(std::make_shared<geometric::LazyLBTRRT>(si_));
372  else if (plannerName == "LazyPRM")
373  planners_.push_back(std::make_shared<geometric::LazyPRM>(si_));
374  else if (plannerName == "LazyPRMstar")
375  planners_.push_back(std::make_shared<geometric::LazyPRMstar>(si_));
376  else if (plannerName == "LazyRRT")
377  planners_.push_back(std::make_shared<geometric::LazyRRT>(si_));
378  else if (plannerName == "LBKPIECE")
379  planners_.push_back(std::make_shared<geometric::LBKPIECE1>(si_));
380  else if (plannerName == "LBTRRT")
381  planners_.push_back(std::make_shared<geometric::LBTRRT>(si_));
382  else if (plannerName == "PDST")
383  planners_.push_back(std::make_shared<geometric::PDST>(si_));
384  else if (plannerName == "PRM")
385  planners_.push_back(std::make_shared<geometric::PRM>(si_));
386  else if (plannerName == "PRMstar")
387  planners_.push_back(std::make_shared<geometric::PRMstar>(si_));
388  else if (plannerName == "ProjEST")
389  planners_.push_back(std::make_shared<geometric::ProjEST>(si_));
390  else if (plannerName == "RRT")
391  planners_.push_back(std::make_shared<geometric::RRT>(si_));
392  else if (plannerName == "RRTConnect")
393  planners_.push_back(std::make_shared<geometric::RRTConnect>(si_));
394  else if (plannerName == "RRTstar")
395  planners_.push_back(std::make_shared<geometric::RRTstar>(si_));
396  else if (plannerName == "RRTXstatic")
397  planners_.push_back(std::make_shared<geometric::RRTXstatic>(si_));
398  else if (plannerName == "SBL")
399  planners_.push_back(std::make_shared<geometric::SBL>(si_));
400  else if (plannerName == "SPARS")
401  planners_.push_back(std::make_shared<geometric::SPARS>(si_));
402  else if (plannerName == "SPARStwo")
403  planners_.push_back(std::make_shared<geometric::SPARStwo>(si_));
404  else if (plannerName == "SST")
405  planners_.push_back(std::make_shared<geometric::SST>(si_));
406  else if (plannerName == "STRIDE")
407  planners_.push_back(std::make_shared<geometric::STRIDE>(si_));
408  else if (plannerName == "TRRT")
409  planners_.push_back(std::make_shared<geometric::TRRT>(si_));
410  else
411  OMPL_ERROR("Unknown planner name: %s", plannerName.c_str());
412 
413  std::vector<std::string> paramValue;
414  for (auto it = plannerAndParams.begin() + 1; it != plannerAndParams.end(); ++it)
415  if (!it->empty())
416  {
417  boost::split(paramValue, *it, boost::is_any_of("="));
418  planners_.back()->params().setParam(paramValue[0], paramValue[1]);
419  }
420  }
421 }
422 
424 {
425  std::stringstream ss;
426  for (unsigned int i = 0; i < planners_.size(); ++i)
427  {
428  if (i > 0)
429  ss << ',';
430  ss << planners_[i]->getName();
431 
432  std::map<std::string, std::string> params;
433  planners_[i]->params().getParams(params);
434  if (params.size() > 0)
435  {
436  ss << '[';
437  for (auto it = params.begin(); it != params.end(); ++it)
438  {
439  if (it != params.begin())
440  ss << ' ';
441  ss << it->first << '=' << it->second;
442  }
443  ss << ']';
444  }
445  }
446  return ss.str();
447 }
448 
449 void ompl::geometric::AnytimePathShortening::printSettings(std::ostream &out) const
450 {
451  Planner::printSettings(out);
452  out << "Settings for planner instances in AnytimePathShortening instance:\n";
453  for (const auto &planner : planners_)
454  {
455  out << "* ";
456  planner->printSettings(out);
457  }
458 }
Base class for a planner.
Definition: Planner.h:284
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:261
The planner found an approximate solution.
void getPlannerData(base::PlannerData &data) const override
Get information about the most recent run of the motion planner.
An optimization objective which corresponds to optimizing path length.
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:472
This class contains routines that attempt to simplify geometric paths.
double value() const
The value of the cost.
Definition: Cost.h:148
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:108
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planners. The problem needs to be set before calling solve()....
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:67
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:90
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:235
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...
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...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:491
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:135
LogLevel
The set of priorities for message logging.
Definition: Console.h:83
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:268
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.
A shared pointer wrapper for ompl::base::ProblemDefinition.
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.
Abstract definition of goals.
Definition: Goal.h:123
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:65
unsigned int getDefaultNumPlanners() const
Get default number of planners used if none are specified.
The planner found an exact solution.
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:80
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:141
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:63
void setPlanners(const std::string &plannerList)
Set the list of planners to use.
virtual double length() const =0
Return the length of a path.
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:264
~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:242
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:81
Main namespace. Contains everything in this library.
Definition: AppBase.h:20
void setShortcut(bool shortcut)
Enable/disable shortcutting on paths.
Abstract definition of a path.
Definition: Path.h:128
void checkValidity() override
Check to see if the planners are in a working state (setup has been called, a goal was set,...