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 
44 namespace ompl
45 {
46  namespace geometric
47  {
68  class AnytimePathShortening : public base::Planner
69  {
70  public:
73  template<typename PlannerType>
74  static std::shared_ptr<AnytimePathShortening> createPlanner(
75  const base::SpaceInformationPtr &si,
76  unsigned int numPlanners = std::max(1u, std::thread::hardware_concurrency()))
77  {
78  auto result = std::make_shared<AnytimePathShortening>(si);
79  result->planners_.reserve(numPlanners);
80  for (unsigned int i = 0; i < numPlanners; ++i)
81  result->planners_.emplace_back(std::make_shared<PlannerType>(si));
82  return result;
83  }
91  template<typename ... PlannerTypes>
92  static std::shared_ptr<AnytimePathShortening> createPlanner(const base::SpaceInformationPtr &si)
93  {
94  auto result = std::make_shared<AnytimePathShortening>(si);
95  result->planners_ = std::vector<base::PlannerPtr>{std::make_shared<PlannerTypes>(si)...};
96  return result;
97  }
98 
100  AnytimePathShortening(const base::SpaceInformationPtr &si);
101 
103  ~AnytimePathShortening() override;
104 
107  void addPlanner(base::PlannerPtr &planner);
108 
113  void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override;
114 
121  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
122 
126  void clear() override;
127 
131  void getPlannerData(base::PlannerData &data) const override;
132 
134  virtual void getPlannerData(ompl::base::PlannerData &data, unsigned int idx) const;
135 
139  void setup() override;
140 
145  void checkValidity() override;
146 
148  unsigned int getNumPlanners() const;
149 
151  base::PlannerPtr getPlanner(unsigned int idx) const;
152 
154  bool isShortcutting() const;
155 
157  void setShortcut(bool shortcut);
158 
160  bool isHybridizing() const;
161 
163  void setHybridize(bool hybridize);
164 
166  unsigned int maxHybridizationPaths() const;
167 
169  void setMaxHybridizationPath(unsigned int maxPathCount);
170 
179  void setPlanners(const std::string &plannerList);
180 
182  std::string getPlanners() const;
183 
185  void setDefaultNumPlanners(unsigned int numPlanners);
186 
188  unsigned int getDefaultNumPlanners() const;
189 
191  std::string getBestCost() const;
192 
194  void printSettings(std::ostream &out) const override;
195 
196  protected:
199  virtual void threadSolve(base::Planner *planner, const base::PlannerTerminationCondition &ptc);
200 
202  std::vector<base::PlannerPtr> planners_;
203 
205  bool shortcut_{true};
206 
208  bool hybridize_{true};
209 
212  unsigned int maxHybridPaths_{24};
213 
216  unsigned int defaultNumPlanners_;
217  };
218  }
219 }
220 #endif
bool hybridize_
Flag indicating whether to hybridize the set of solution paths.
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.
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.
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planners. The problem needs to be set before calling solve()....
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.
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:235
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 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:20
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.