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
45namespace ompl
46{
47 namespace geometric
48 {
50 OMPL_CLASS_FORWARD(PathGeometric);
52
74
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
117
120 void addPlanner(base::PlannerPtr &planner);
121
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
231
233 base::Cost bestCost_{std::numeric_limits<double>::quiet_NaN()};
234
236 std::mutex lock_;
237 };
238 }
239}
240#endif
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
bool hybridize_
Flag indicating whether to hybridize the set of solution paths.
base::PlannerPtr getPlanner(unsigned int idx) const
Retrieve a pointer to the ith planner instance.
~AnytimePathShortening() override
Destructor.
unsigned int defaultNumPlanners_
The number of planners to use if none are specified. This defaults to the number of cores....
void clear() override
Clear all internal planning datastructures. Planner settings are not affected. Subsequent calls to so...
std::string getBestCost() const
Return best cost found so far by algorithm.
void setPlanners(const std::string &plannerList)
Set the list of planners to use.
void getPlannerData(base::PlannerData &data) const override
Get information about the most recent run of the motion planner.
unsigned int maxHybridizationPaths() const
Return the maximum number of paths that will be hybridized.
virtual void threadSolve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
The function that the planning threads execute when solving a motion planning problem.
void setDefaultNumPlanners(unsigned int numPlanners)
Set default number of planners to use if none are specified.
static std::shared_ptr< AnytimePathShortening > createPlanner(const base::SpaceInformationPtr &si)
Factory for creating a shared pointer to an AnytimePathShortening instance with planners of type Plan...
bool shortcut_
Flag indicating whether to shortcut paths.
unsigned int maxHybridPaths_
The maximum number of paths that will be hybridized. This prohibits hybridization of a very large pat...
AnytimePathShortening(const base::SpaceInformationPtr &si)
Constructor requires the space information to plan in.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Method that solves the motion planning problem. This method terminates under just two conditions,...
void printSettings(std::ostream &out) const override
Print settings of this planner as well as those of the planner instances it contains.
std::mutex lock_
mutex for updating bestCost_
void setup() override
Perform any necessary configuration steps. This method also invokes ompl::base::SpaceInformation::set...
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...
bool isShortcutting() const
Return whether the anytime planner will perform shortcutting on paths.
bool isHybridizing() const
Return whether the anytime planner will extract a hybrid path from the set of solution paths.
void setMaxHybridizationPath(unsigned int maxPathCount)
Set 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 addPath(const geometric::PathGeometricPtr &path, base::Planner *planner)
add a path to set of solutions
unsigned int getNumPlanners() const
Retrieve the number of planners added.
void addPlanner(base::PlannerPtr &planner)
Adds the given planner to the set of planners used to compute candidate paths.
base::Cost bestCost_
Best cost found so far by algorithm.
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.
std::vector< base::PlannerPtr > planners_
The list of planners used for solving the problem.
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,...
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49