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 #include "ompl/util/String.h"
43 
44 #include <thread>
45 
47  : ompl::base::Planner(si, "APS")
48  , defaultNumPlanners_(std::max(1u, std::thread::hardware_concurrency()))
49 {
51  specs_.multithreaded = true;
52  specs_.optimizingPaths = true;
53 
54  Planner::declareParam<bool>("shortcut", this, &AnytimePathShortening::setShortcut,
56  Planner::declareParam<bool>("hybridize", this, &AnytimePathShortening::setHybridize,
58  Planner::declareParam<unsigned int>("max_hybrid_paths", this, &AnytimePathShortening::setMaxHybridizationPath,
60  Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners,
62 
63  addPlannerProgressProperty("best cost REAL", [this]
64  {
65  return getBestCost();
66  });
67 }
68 
70 
72 {
73  if (planner && planner->getSpaceInformation().get() != si_.get())
74  {
75  OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
76  return;
77  }
78 
79  // Ensure all planners are unique instances
80  for (auto &i : planners_)
81  {
82  if (planner.get() == i.get())
83  {
84  OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
85  return;
86  }
87  }
88 
89  planners_.push_back(planner);
90 }
91 
93 {
95  for (auto &planner : planners_)
96  planner->setProblemDefinition(pdef);
97 }
98 
101 {
102  base::Goal *goal = pdef_->getGoal().get();
103  std::vector<std::thread *> threads(planners_.size());
104  geometric::PathHybridization phybrid(si_);
105  base::Path *bestSln = nullptr;
106 
107  base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
108  if (!opt)
109  {
110  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
111  "planning time.",
112  getName().c_str());
113  opt = std::make_shared<base::PathLengthOptimizationObjective>(si_);
114  pdef_->setOptimizationObjective(opt);
115  }
116  else
117  {
118  if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt.get()) == nullptr)
119  OMPL_WARN("The optimization objective is not set for path length. The specified optimization criteria may "
120  "not be optimized over.");
121  }
122 
123  // Disable output from the motion planners, except for errors
124  msg::LogLevel currentLogLevel = msg::getLogLevel();
125  msg::setLogLevel(std::max(msg::LOG_ERROR, currentLogLevel));
126  while (!ptc())
127  {
128  // We have found a solution that is good enough
129  if ((bestSln != nullptr) && opt->isSatisfied(base::Cost(bestSln->length())))
130  break;
131 
132  // Clear any previous planning data for the set of planners
133  clear();
134 
135  // Spawn a thread for each planner. This will shortcut the best path after solving.
136  for (size_t i = 0; i < threads.size(); ++i)
137  threads[i] = new std::thread([this, i, &ptc]
138  {
139  return threadSolve(planners_[i].get(), ptc);
140  });
141 
142  // Join each thread, and then delete it
143  for (auto &thread : threads)
144  {
145  thread->join();
146  delete thread;
147  }
148 
149  // Hybridize the set of paths computed. Add the new hybrid path to the mix.
150  if (hybridize_ && !ptc())
151  {
152  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
153  for (size_t j = 0; j < paths.size() && j < maxHybridPaths_ && !ptc(); ++j)
154  phybrid.recordPath(paths[j].path_, false);
155 
156  phybrid.computeHybridPath();
157  const base::PathPtr &hsol = phybrid.getHybridPath();
158  if (hsol)
159  {
160  auto *pg = static_cast<geometric::PathGeometric *>(hsol.get());
161  double difference = 0.0;
162  bool approximate = !goal->isSatisfied(pg->getStates().back(), &difference);
163  pdef_->addSolutionPath(hsol, approximate, difference);
164  }
165  }
166 
167  // keep track of the best solution found so far
168  if (pdef_->getSolutionCount() > 0)
169  bestSln = pdef_->getSolutionPath().get();
170  }
171  msg::setLogLevel(currentLogLevel);
172 
173  if (bestSln != nullptr)
174  {
175  if (goal->isSatisfied(static_cast<geometric::PathGeometric *>(bestSln)->getStates().back()))
178  }
180 }
181 
184 {
185  // compute a motion plan
186  base::PlannerStatus status = planner->solve(ptc);
187 
188  // Shortcut the best solution found so far
189  if (shortcut_ && status == base::PlannerStatus::EXACT_SOLUTION)
190  {
191  geometric::PathGeometric *sln = static_cast<geometric::PathGeometric *>(pdef_->getSolutionPath().get());
192  auto pathCopy(std::make_shared<geometric::PathGeometric>(*sln));
193  geometric::PathSimplifier ps(pdef_->getSpaceInformation());
194  if (ps.shortcutPath(*pathCopy))
195  {
196  double difference = 0.0;
197  bool approximate = !pdef_->getGoal()->isSatisfied(pathCopy->getStates().back(), &difference);
198  pdef_->addSolutionPath(pathCopy, approximate, difference);
199  }
200  }
201 }
202 
204 {
205  Planner::clear();
206  for (auto &planner : planners_)
207  planner->clear();
208 }
209 
211 {
212  if (planners_.empty())
213  return;
214 
215  OMPL_WARN("Returning planner data for planner #0");
216  getPlannerData(data, 0);
217 }
218 
220 {
221  if (planners_.size() < idx)
222  return;
223  planners_[idx]->getPlannerData(data);
224 }
225 
227 {
228  Planner::setup();
229 
230  if (planners_.empty())
231  {
232  planners_.reserve(defaultNumPlanners_);
233  for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
234  {
235  planners_.push_back(tools::SelfConfig::getDefaultPlanner(pdef_->getGoal()));
236  planners_.back()->setProblemDefinition(pdef_);
237  }
238  OMPL_INFORM("%s: No planners specified; using %u instances of %s", getName().c_str(), planners_.size(),
239  planners_[0]->getName().c_str());
240  }
241 
242  for (auto &planner : planners_)
243  planner->setup();
244 }
245 
247 {
248  for (auto &planner : planners_)
249  planner->checkValidity();
250 }
251 
253 {
254  return planners_.size();
255 }
256 
258 {
259  assert(idx < planners_.size());
260  return planners_[idx];
261 }
262 
264 {
265  return shortcut_;
266 }
267 
269 {
270  shortcut_ = shortcut;
271 }
272 
274 {
275  return hybridize_;
276 }
277 
279 {
280  hybridize_ = hybridize;
281 }
282 
284 {
285  return maxHybridPaths_;
286 }
287 
289 {
290  maxHybridPaths_ = maxPathCount;
291 }
292 
294 {
295  defaultNumPlanners_ = numPlanners;
296 }
297 
299 {
300  return defaultNumPlanners_;
301 }
302 
304 {
305  base::Cost bestCost(std::numeric_limits<double>::quiet_NaN());
306  if (pdef_ && pdef_->getSolutionCount() > 0)
307  bestCost = base::Cost(pdef_->getSolutionPath()->length());
308  return ompl::toString(bestCost.value());
309 }
Base class for a planner.
Definition: Planner.h:287
unsigned int maxHybridizationPaths() const
Return the maximum number of paths that will be hybridized.
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:264
@ APPROXIMATE_SOLUTION
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:463
unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
This class contains routines that attempt to simplify geometric paths.
double value() const
The value of the cost.
Definition: Cost.h:152
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
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: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:92
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
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...
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:482
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.
const base::PathPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:271
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.
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
A shared pointer wrapper for ompl::base::ProblemDefinition.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
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:126
#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.
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:76
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
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:267
~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.
void setShortcut(bool shortcut)
Enable/disable shortcutting on paths.
Abstract definition of a path.
Definition: Path.h:131
void checkValidity() override
Check to see if the planners are in a working state (setup has been called, a goal was set,...