AnytimePathShortening.h
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 #ifndef OMPL_GEOMETRIC_PLANNERS_ANYTIMEOPTIMIZATION_ANYTIMEPATHSHORTENING_
38 #define OMPL_GEOMETRIC_PLANNERS_ANYTIMEOPTIMIZATION_ANYTIMEPATHSHORTENING_
39 
40 #include "ompl/base/Planner.h"
41 #include <vector>
42 #include <thread>
43 #include <mutex>
44 
45 namespace ompl
46 {
47  namespace geometric
48  {
50  OMPL_CLASS_FORWARD(PathGeometric);
52 
74 
81  class AnytimePathShortening : public base::Planner
82  {
83  public:
86  template<typename PlannerType>
87  static std::shared_ptr<AnytimePathShortening> createPlanner(
88  const base::SpaceInformationPtr &si,
89  unsigned int numPlanners = std::max(1u, std::thread::hardware_concurrency()))
90  {
91  auto result = std::make_shared<AnytimePathShortening>(si);
92  result->planners_.reserve(numPlanners);
93  for (unsigned int i = 0; i < numPlanners; ++i)
94  result->planners_.emplace_back(std::make_shared<PlannerType>(si));
95  return result;
96  }
104  template<typename ... PlannerTypes>
105  static std::shared_ptr<AnytimePathShortening> createPlanner(const base::SpaceInformationPtr &si)
106  {
107  auto result = std::make_shared<AnytimePathShortening>(si);
108  result->planners_ = std::vector<base::PlannerPtr>{std::make_shared<PlannerTypes>(si)...};
109  return result;
110  }
111 
113  AnytimePathShortening(const base::SpaceInformationPtr &si);
114 
116  ~AnytimePathShortening() override;
117 
120  void addPlanner(base::PlannerPtr &planner);
121 
128  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
129 
133  void clear() override;
134 
138  void getPlannerData(base::PlannerData &data) const override;
139 
141  virtual void getPlannerData(ompl::base::PlannerData &data, unsigned int idx) const;
142 
146  void setup() override;
147 
152  void checkValidity() override;
153 
155  unsigned int getNumPlanners() const;
156 
158  base::PlannerPtr getPlanner(unsigned int idx) const;
159 
161  bool isShortcutting() const;
162 
164  void setShortcut(bool shortcut);
165 
167  bool isHybridizing() const;
168 
170  void setHybridize(bool hybridize);
171 
173  unsigned int maxHybridizationPaths() const;
174 
176  void setMaxHybridizationPath(unsigned int maxPathCount);
177 
186  void setPlanners(const std::string &plannerList);
187 
189  std::string getPlanners() const;
190 
192  void setDefaultNumPlanners(unsigned int numPlanners);
193 
195  unsigned int getDefaultNumPlanners() const;
196 
198  std::string getBestCost() const;
199 
201  void printSettings(std::ostream &out) const override;
202 
203  protected:
209  void addPath(const geometric::PathGeometricPtr &path, base::Planner *planner);
210 
213  virtual void threadSolve(base::Planner *planner, const base::PlannerTerminationCondition &ptc);
214 
216  std::vector<base::PlannerPtr> planners_;
217 
219  bool shortcut_{true};
220 
222  bool hybridize_{true};
223 
226  unsigned int maxHybridPaths_{24};
227 
230  unsigned int defaultNumPlanners_;
231 
233  base::Cost bestCost_{std::numeric_limits<double>::quiet_NaN()};
234 
236  std::mutex lock_;
237  };
238  }
239 }
240 #endif
bool hybridize_
Flag indicating whether to hybridize the set of solution paths.
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.
void getPlannerData(base::PlannerData &data) const override
Get information about the most recent run of the motion planner.
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::Cost bestCost_
Best cost found so far by algorithm.
static std::shared_ptr< AnytimePathShortening > createPlanner(const base::SpaceInformationPtr &si, unsigned int numPlanners=std::max(1u, std::thread::hardware_concurrency()))
Factory for creating a shared pointer to an AnytimePathShortening instance with numPlanners instances...
base::PlannerPtr getPlanner(unsigned int idx) const
Retrieve a pointer to the ith planner instance.
std::mutex lock_
mutex for updating bestCost_
unsigned int getNumPlanners() const
Retrieve the number of planners added.
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
AnytimePathShortening(const base::SpaceInformationPtr &si)
Constructor requires the space information to plan in.
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.
bool shortcut_
Flag indicating whether to shortcut paths.
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.
unsigned int getDefaultNumPlanners() const
Get default number of planners used if none are specified.
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...
void setPlanners(const std::string &plannerList)
Set the list of planners to use.
~AnytimePathShortening() override
Destructor.
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,...
unsigned int maxHybridPaths_
The maximum number of paths that will be hybridized. This prohibits hybridization of a very large pat...
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,...
std::vector< base::PlannerPtr > planners_
The list of planners used for solving the problem.