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 <thread>
44 
46  : ompl::base::Planner(si, "APS")
47  , shortcut_(true)
48  , hybridize_(true)
49  , maxHybridPaths_(24)
50  , defaultNumPlanners_(std::max(1u, std::thread::hardware_concurrency()))
51 {
53  specs_.multithreaded = true;
54  specs_.optimizingPaths = true;
55 
56  Planner::declareParam<bool>("shortcut", this, &AnytimePathShortening::setShortcut,
58  Planner::declareParam<bool>("hybridize", this, &AnytimePathShortening::setHybridize,
60  Planner::declareParam<unsigned int>("max_hybrid_paths", this, &AnytimePathShortening::setMaxHybridizationPath,
62  Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners,
64 
65  addPlannerProgressProperty("best cost REAL", [this]
66  {
67  return getBestCost();
68  });
69 }
70 
72 
74 {
75  if (planner && planner->getSpaceInformation().get() != si_.get())
76  {
77  OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
78  return;
79  }
80 
81  // Ensure all planners are unique instances
82  for (auto &i : planners_)
83  {
84  if (planner.get() == i.get())
85  {
86  OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
87  return;
88  }
89  }
90 
91  planners_.push_back(planner);
92 }
93 
95 {
97  for (auto &planner : planners_)
98  planner->setProblemDefinition(pdef);
99 }
100 
103 {
104  base::Goal *goal = pdef_->getGoal().get();
105  std::vector<std::thread *> threads(planners_.size());
107  base::Path *bestSln = nullptr;
108 
109  base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
110  if (!opt)
111  {
112  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
113  "planning time.",
114  getName().c_str());
115  opt = std::make_shared<base::PathLengthOptimizationObjective>(si_);
116  pdef_->setOptimizationObjective(opt);
117  }
118  else
119  {
120  if (!dynamic_cast<base::PathLengthOptimizationObjective *>(opt.get()))
121  OMPL_WARN("The optimization objective is not set for path length. The specified optimization criteria may "
122  "not be optimized over.");
123  }
124 
125  // Disable output from the motion planners, except for errors
126  msg::LogLevel currentLogLevel = msg::getLogLevel();
127  msg::setLogLevel(std::max(msg::LOG_ERROR, currentLogLevel));
128  while (!ptc())
129  {
130  // We have found a solution that is good enough
131  if (bestSln && opt->isSatisfied(base::Cost(bestSln->length())))
132  break;
133 
134  // Clear any previous planning data for the set of planners
135  clear();
136 
137  // Spawn a thread for each planner. This will shortcut the best path after solving.
138  for (size_t i = 0; i < threads.size(); ++i)
139  threads[i] = new std::thread([this, i, &ptc]
140  {
141  return threadSolve(planners_[i].get(), ptc);
142  });
143 
144  // Join each thread, and then delete it
145  for (auto &thread : threads)
146  {
147  thread->join();
148  delete thread;
149  }
150 
151  // Hybridize the set of paths computed. Add the new hybrid path to the mix.
152  if (hybridize_ && !ptc())
153  {
154  const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
155  for (size_t j = 0; j < paths.size() && j < maxHybridPaths_ && !ptc(); ++j)
156  phybrid.recordPath(paths[j].path_, false);
157 
158  phybrid.computeHybridPath();
159  const base::PathPtr &hsol = phybrid.getHybridPath();
160  if (hsol)
161  {
162  geometric::PathGeometric *pg = static_cast<geometric::PathGeometric *>(hsol.get());
163  double difference = 0.0;
164  bool approximate = !goal->isSatisfied(pg->getStates().back(), &difference);
165  pdef_->addSolutionPath(hsol, approximate, difference);
166  }
167  }
168 
169  // keep track of the best solution found so far
170  if (pdef_->getSolutionCount() > 0)
171  bestSln = pdef_->getSolutionPath().get();
172  }
173  msg::setLogLevel(currentLogLevel);
174 
175  if (bestSln)
176  {
177  if (goal->isSatisfied(static_cast<geometric::PathGeometric *>(bestSln)->getStates().back()))
180  }
182 }
183 
186 {
187  // compute a motion plan
188  base::PlannerStatus status = planner->solve(ptc);
189 
190  // Shortcut the best solution found so far
192  {
193  geometric::PathGeometric *sln = static_cast<geometric::PathGeometric *>(pdef_->getSolutionPath().get());
194  auto pathCopy(std::make_shared<geometric::PathGeometric>(*sln));
195  geometric::PathSimplifier ps(pdef_->getSpaceInformation());
196  if (ps.shortcutPath(*pathCopy))
197  {
198  double difference = 0.0;
199  bool approximate = !pdef_->getGoal()->isSatisfied(pathCopy->getStates().back(), &difference);
200  pdef_->addSolutionPath(pathCopy, approximate, difference);
201  }
202  }
203 }
204 
206 {
207  Planner::clear();
208  for (auto &planner : planners_)
209  planner->clear();
210 }
211 
213 {
214  if (planners_.size() == 0)
215  return;
216 
217  OMPL_WARN("Returning planner data for planner #0");
218  getPlannerData(data, 0);
219 }
220 
222 {
223  if (planners_.size() < idx)
224  return;
225  planners_[idx]->getPlannerData(data);
226 }
227 
229 {
230  Planner::setup();
231 
232  if (planners_.size() == 0)
233  {
235  for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
236  {
238  planners_.back()->setProblemDefinition(pdef_);
239  }
240  OMPL_INFORM("%s: No planners specified; using %u instances of %s", getName().c_str(), planners_.size(),
241  planners_[0]->getName().c_str());
242  }
243 
244  for (auto &planner : planners_)
245  planner->setup();
246 }
247 
249 {
250  for (auto &planner : planners_)
251  planner->checkValidity();
252 }
253 
255 {
256  return planners_.size();
257 }
258 
260 {
261  assert(idx < planners_.size());
262  return planners_[idx];
263 }
264 
266 {
267  return shortcut_;
268 }
269 
271 {
272  shortcut_ = shortcut;
273 }
274 
276 {
277  return hybridize_;
278 }
279 
281 {
282  hybridize_ = hybridize;
283 }
284 
286 {
287  return maxHybridPaths_;
288 }
289 
291 {
292  maxHybridPaths_ = maxPathCount;
293 }
294 
296 {
297  defaultNumPlanners_ = numPlanners;
298 }
299 
301 {
302  return defaultNumPlanners_;
303 }
304 
306 {
307  base::Cost bestCost(std::numeric_limits<double>::quiet_NaN());
308  if (pdef_ && pdef_->getSolutionCount() > 0)
309  bestCost = base::Cost(pdef_->getSolutionPath()->length());
310  return std::to_string(bestCost.value());
311 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
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:408
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
bool isHybridizing() const
Return whether the anytime planner will extract a hybrid path from the set of solution paths...
base::PlannerPtr getPlanner(unsigned int i) const
Retrieve a pointer to the ith planner instance.
A shared pointer wrapper for ompl::base::ProblemDefinition.
void setMaxHybridizationPath(unsigned int maxPathCount)
Set the maximum number of paths that will be hybridized.
~AnytimePathShortening() override
Destructor.
Abstract definition of goals.
Definition: Goal.h:62
void clear() override
Clear all internal planning datastructures. Planner settings are not affected. Subsequent calls to so...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
void getPlannerData(base::PlannerData &data) const override
Get information about the most recent run of the motion planner.
AnytimePathShortening(const base::SpaceInformationPtr &si)
Constructor requires the space information to plan in.
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:209
unsigned int getDefaultNumPlanners() const
Get default number of planners used if none are specified.
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.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
unsigned int maxHybridPaths_
The maximum number of paths that will be hybridized. This prohibits hybridization of a very large pat...
A shared pointer wrapper for ompl::base::Planner.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Base class for a planner.
Definition: Planner.h:232
void checkValidity() override
Check to see if the planners are in a working state (setup has been called, a goal was set...
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:142
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:136
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
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 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:76
void setup() override
Perform any necessary configuration steps. This method also invokes ompl::base::SpaceInformation::set...
unsigned int defaultNumPlanners_
The number of planners to use if none are specified. This defaults to the number of cores...
bool isShortcutting() const
Return whether the anytime planner will perform shortcutting on paths.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Method that solves the motion planning problem. This method terminates under just two conditions...
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:427
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:243
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 shared pointer wrapper for ompl::base::OptimizationObjective.
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
void addPlanner(base::PlannerPtr &planner)
Adds the given planner to the set of planners used to compute candidate paths.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:216
std::vector< base::PlannerPtr > planners_
The list of planners used for solving the problem.
double value() const
The value of the cost.
Definition: Cost.h:56
unsigned int getNumPlanners() const
Retrieve the number of planners added.
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planners. The problem needs to be set before calling solve()...
Definition of a geometric path.
Definition: PathGeometric.h:60
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
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:84
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
A shared pointer wrapper for ompl::base::Path.
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...
unsigned int maxHybridizationPaths() const
Return the maximum number of paths that will be hybridized.
bool shortcut_
Flag indicating whether to shortcut paths.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68