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 
41 #include "ompl/geometric/planners/cforest/CForestStateSpaceWrapper.h"
42 #include "ompl/geometric/planners/PlannerIncludes.h"
43 #include "ompl/tools/config/SelfConfig.h"
44 
45 #include <boost/thread.hpp>
46 
47 #include <vector>
48 
49 namespace ompl
50 {
51 
52  namespace geometric
53  {
54 
78  class CForest : public base::Planner
79  {
80  public:
81 
83 
84  virtual ~CForest();
85 
86  virtual void getPlannerData(base::PlannerData &data) const;
87 
88  virtual void clear();
89 
91  template <class T>
93  {
94  base::CForestStateSpaceWrapper *cfspace = new base::CForestStateSpaceWrapper(this, si_->getStateSpace().get());
95  base::StateSpacePtr space(cfspace);
97  si->setStateValidityChecker(si_->getStateValidityChecker());
98  si->setMotionValidator(si_->getMotionValidator());
99  base::PlannerPtr planner(new T(si));
100  cfspace->setPlanner(planner.get());
101  addPlannerInstanceInternal(planner);
102  }
103 
105  template <class T>
106  void addPlannerInstances(std::size_t num = 2)
107  {
108  planners_.reserve(planners_.size() + num);
109  for (std::size_t i = 0 ; i < num; ++i)
110  {
111  addPlannerInstance<T>();
112  }
113  }
114 
117  {
118  planners_.clear();
119  }
121  base::PlannerPtr& getPlannerInstance(const std::size_t idx)
122  {
123  return planners_[idx];
124  }
125 
126  virtual void setup();
127 
129 
130  void addSampler(base::StateSamplerPtr sampler)
131  {
132  addSamplerMutex_.lock();
133  samplers_.push_back(sampler);
134  addSamplerMutex_.unlock();
135  }
136 
138  void setPrune(const bool prune)
139  {
140  prune_ = prune;
141  }
142 
144  bool getPrune() const
145  {
146  return prune_;
147  }
148 
150  void setNumThreads(unsigned int numThreads = 0);
151 
153  unsigned int getNumThreads()
154  {
155  return numThreads_;
156  }
157 
159  std::string getBestCost() const;
160 
162  std::string getNumPathsShared() const;
163 
165  std::string getNumStatesShared() const;
166 
167  private:
168 
170  void addPlannerInstanceInternal(const base::PlannerPtr &planner);
171 
173  void newSolutionFound(const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost);
174 
175  protected:
176 
178  void solve(base::Planner *planner, const base::PlannerTerminationCondition &ptc);
179 
182 
184  std::vector<base::PlannerPtr> planners_;
185 
187  std::vector<base::StateSamplerPtr> samplers_;
188 
190  boost::unordered_set<const base::State *> statesShared_;
191 
194 
196  unsigned int numPathsShared_;
197 
199  unsigned int numStatesShared_;
200 
203 
205  boost::mutex addSamplerMutex_;
206 
208  bool prune_;
209 
211  unsigned int numThreads_;
212  };
213  }
214 }
215 
216 #endif
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
std::string getNumStatesShared() const
Get number of states actually shared by the algorithm.
Definition: CForest.cpp:204
boost::unordered_set< const base::State * > statesShared_
Stores the states already shared to check if a specific state has been shared.
Definition: CForest.h:190
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost 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:211
std::vector< base::StateSamplerPtr > samplers_
The set of sampler allocated by the planners.
Definition: CForest.h:187
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: CForest.cpp:137
void addPlannerInstances(std::size_t num=2)
Add an specific planner instance.
Definition: CForest.h:106
State space wrapper to use together with CForest. It adds some functionalities to the regular state s...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: CForest.cpp:84
boost::mutex newSolutionFoundMutex_
Mutex to control the access to the newSolutionFound() method.
Definition: CForest.h:202
unsigned int numPathsShared_
Number of paths shared among threads.
Definition: CForest.h:196
base::Cost bestCost_
Cost of the best path found so far among planners.
Definition: CForest.h:193
base::OptimizationObjectivePtr opt_
Optimization objective taken into account when planning.
Definition: CForest.h:181
std::string getBestCost() const
Get best cost among all the planners.
Definition: CForest.cpp:194
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:66
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: CForest.cpp:119
Main namespace. Contains everything in this library.
Definition: Cost.h:42
unsigned int numStatesShared_
Number of states shared among threads.
Definition: CForest.h:199
A boost shared pointer wrapper for ompl::base::Planner.
Base class for a planner.
Definition: Planner.h:232
bool getPrune() const
Get the state of the pruning option.
Definition: CForest.h:144
bool prune_
Flag to control the tree pruning.
Definition: CForest.h:208
boost::mutex addSamplerMutex_
Mutex to control the access to samplers_.
Definition: CForest.h:205
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: CForest.cpp:164
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
The base class for space information. This contains all the information about the space planning is d...
std::vector< base::PlannerPtr > planners_
The set of planners to be used.
Definition: CForest.h:184
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
std::string getNumPathsShared() const
Get number of paths shared by the algorithm.
Definition: CForest.cpp:199
Coupled Forest of Random Engrafting Search Trees.
Definition: CForest.h:78
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
void setPrune(const bool prune)
Option to control whether the tree is pruned during the search.
Definition: CForest.h:138
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:116
unsigned int getNumThreads()
Get default number of threads used by CForest when no planner instances are specified by the user...
Definition: CForest.h:153
void addPlannerInstance()
Add an specific planner instance.
Definition: CForest.h:92
base::PlannerPtr & getPlannerInstance(const std::size_t idx)
Return an specific planner instance.
Definition: CForest.h:121