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 
48 namespace ompl
49 {
50  namespace geometric
51  {
75  class CForest : public base::Planner
76  {
77  public:
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  const base::Cost cost);
172 
173  protected:
175  void solve(base::Planner *planner, const base::PlannerTerminationCondition &ptc);
176 
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_;
194 
196  unsigned int numStatesShared_;
197 
200 
202  std::mutex addSamplerMutex_;
203 
206 
208  unsigned int numThreads_;
209  };
210  }
211 }
212 
213 #endif
bool getFocusSearch() const
Get the state of the search focusing option.
Definition: CForest.h:141
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
std::string getNumStatesShared() const
Get number of states actually shared by the algorithm.
Definition: CForest.cpp:227
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
A shared pointer wrapper for ompl::base::StateSampler.
unsigned int numThreads_
Default number of threads to use when no planner instances are specified by the user.
Definition: CForest.h:208
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:176
std::vector< base::StateSamplerPtr > samplers_
The set of sampler allocated by the planners.
Definition: CForest.h:184
void addPlannerInstances(std::size_t num=2)
Add specific planner instances. CFOREST sets the planner&#39;s parameter named focus_search (if present) ...
Definition: CForest.h:103
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: CForest.cpp:127
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int numPathsShared_
Number of paths shared among threads.
Definition: CForest.h:193
base::Cost bestCost_
Cost of the best path found so far among planners.
Definition: CForest.h:190
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: CForest.cpp:145
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:217
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:71
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
unsigned int numStatesShared_
Number of states shared among threads.
Definition: CForest.h:196
A shared pointer wrapper for ompl::base::Planner.
Base class for a planner.
Definition: Planner.h:232
bool focusSearch_
Flag to control whether the search is focused.
Definition: CForest.h:205
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A shared pointer wrapper for ompl::base::SpaceInformation.
std::vector< base::PlannerPtr > planners_
The set of planners to be used.
Definition: CForest.h:181
void setFocusSearch(const bool focus)
Option to control whether the search is focused during the search.
Definition: CForest.h:135
A shared pointer wrapper for ompl::base::OptimizationObjective.
std::string getNumPathsShared() const
Get number of paths shared by the algorithm.
Definition: CForest.cpp:222
Coupled Forest of Random Engrafting Search Trees.
Definition: CForest.h:75
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:92
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
std::mutex addSamplerMutex_
Mutex to control the access to samplers_.
Definition: CForest.h:202
std::mutex newSolutionFoundMutex_
Mutex to control the access to the newSolutionFound() method.
Definition: CForest.h:199
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void clearPlannerInstances()
Remove all planner instances.
Definition: CForest.h:113
unsigned int getNumThreads()
Get default number of threads used by CForest when no planner instances are specified by the user...
Definition: CForest.h:151
void addPlannerInstance()
Add an specific planner instance.
Definition: CForest.h:88
base::PlannerPtr & getPlannerInstance(const std::size_t idx)
Return an specific planner instance.
Definition: CForest.h:118