Loading...
Searching...
No Matches
CForest.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/* Authors: Javier V. Gómez, Ioan Sucan, Mark Moll */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_CFOREST_CFOREST_
38#define OMPL_GEOMETRIC_PLANNERS_CFOREST_CFOREST_
39
40#include "ompl/geometric/planners/cforest/CForestStateSpaceWrapper.h"
41#include "ompl/geometric/planners/PlannerIncludes.h"
42#include "ompl/tools/config/SelfConfig.h"
43
44#include <mutex>
45
46#include <vector>
47
48namespace ompl
49{
50 namespace geometric
51 {
75 class CForest : public base::Planner
76 {
77 public:
78 CForest(const base::SpaceInformationPtr &si);
79
80 ~CForest() override;
81
82 void getPlannerData(base::PlannerData &data) const override;
83
84 void clear() override;
85
87 template <class T>
89 {
90 auto space(std::make_shared<base::CForestStateSpaceWrapper>(this, si_->getStateSpace().get()));
91 auto si(std::make_shared<base::SpaceInformation>(space));
92 si->setStateValidityChecker(si_->getStateValidityChecker());
93 si->setMotionValidator(si_->getMotionValidator());
94 auto planner(std::make_shared<T>(si));
95 space->setPlanner(planner.get());
96 addPlannerInstanceInternal(planner);
97 }
98
102 template <class T>
103 void addPlannerInstances(std::size_t num = 2)
104 {
105 planners_.reserve(planners_.size() + num);
106 for (std::size_t i = 0; i < num; ++i)
107 {
108 addPlannerInstance<T>();
109 }
110 }
111
114 {
115 planners_.clear();
116 }
118 base::PlannerPtr &getPlannerInstance(const std::size_t idx)
119 {
120 return planners_[idx];
121 }
122
123 void setup() override;
124
126
127 void addSampler(const base::StateSamplerPtr &sampler)
128 {
129 addSamplerMutex_.lock();
130 samplers_.push_back(sampler);
131 addSamplerMutex_.unlock();
132 }
133
135 void setFocusSearch(const bool focus)
136 {
137 focusSearch_ = focus;
138 }
139
141 bool getFocusSearch() const
142 {
143 return focusSearch_;
144 }
145
147 void setNumThreads(unsigned int numThreads = 0);
148
151 unsigned int getNumThreads()
152 {
153 return numThreads_;
154 }
155
157 std::string getBestCost() const;
158
160 std::string getNumPathsShared() const;
161
163 std::string getNumStatesShared() const;
164
165 private:
167 void addPlannerInstanceInternal(const base::PlannerPtr &planner);
168
170 void newSolutionFound(const base::Planner *planner, const std::vector<const base::State *> &states,
171 base::Cost cost);
172
173 protected:
175 void solve(base::Planner *planner, const base::PlannerTerminationCondition &ptc);
176
178 base::OptimizationObjectivePtr opt_;
179
181 std::vector<base::PlannerPtr> planners_;
182
184 std::vector<base::StateSamplerPtr> samplers_;
185
187 std::unordered_set<const base::State *> statesShared_;
188
191
193 unsigned int numPathsShared_{0u};
194
196 unsigned int numStatesShared_{0u};
197
200
203
205 bool focusSearch_{true};
206
208 unsigned int numThreads_;
209 };
210 }
211}
212
213#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,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
Coupled Forest of Random Engrafting Search Trees.
Definition CForest.h:76
unsigned int numThreads_
Default number of threads to use when no planner instances are specified by the user.
Definition CForest.h:208
std::vector< base::StateSamplerPtr > samplers_
The set of sampler allocated by the planners.
Definition CForest.h:184
unsigned int getNumThreads()
Get default number of threads used by CForest when no planner instances are specified by the user.
Definition CForest.h:151
std::vector< base::PlannerPtr > planners_
The set of planners to be used.
Definition CForest.h:181
base::Cost bestCost_
Cost of the best path found so far among planners.
Definition CForest.h:190
base::OptimizationObjectivePtr opt_
Optimization objective taken into account when planning.
Definition CForest.h:178
std::string getBestCost() const
Get best cost among all the planners.
Definition CForest.cpp:213
std::mutex addSamplerMutex_
Mutex to control the access to samplers_.
Definition CForest.h:202
base::PlannerPtr & getPlannerInstance(const std::size_t idx)
Return an specific planner instance.
Definition CForest.h:118
void addPlannerInstances(std::size_t num=2)
Add specific planner instances. CFOREST sets the planner's parameter named focus_search (if present) ...
Definition CForest.h:103
bool getFocusSearch() const
Get the state of the search focusing option.
Definition CForest.h:141
void setNumThreads(unsigned int numThreads=0)
Set default number of threads to use when no planner instances are specified by the user.
Definition CForest.cpp:68
unsigned int numPathsShared_
Number of paths shared among threads.
Definition CForest.h:193
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition CForest.cpp:124
std::unordered_set< const base::State * > statesShared_
Stores the states already shared to check if a specific state has been shared.
Definition CForest.h:187
std::string getNumPathsShared() const
Get number of paths shared by the algorithm.
Definition CForest.cpp:218
std::string getNumStatesShared() const
Get number of states actually shared by the algorithm.
Definition CForest.cpp:223
std::mutex newSolutionFoundMutex_
Mutex to control the access to the newSolutionFound() method.
Definition CForest.h:199
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition CForest.cpp:173
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition CForest.cpp:89
void setFocusSearch(const bool focus)
Option to control whether the search is focused during the search.
Definition CForest.h:135
bool focusSearch_
Flag to control whether the search is focused.
Definition CForest.h:205
void clearPlannerInstances()
Remove all planner instances.
Definition CForest.h:113
void addPlannerInstance()
Add an specific planner instance.
Definition CForest.h:88
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition CForest.cpp:142
unsigned int numStatesShared_
Number of states shared among threads.
Definition CForest.h:196
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()