SelfConfig.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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: Ioan Sucan */
36 
37 #include "ompl/tools/config/SelfConfig.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include "ompl/geometric/planners/rrt/RRTConnect.h"
40 #include "ompl/geometric/planners/rrt/RRT.h"
41 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
42 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
43 #include "ompl/control/planners/rrt/RRT.h"
44 #include "ompl/control/planners/kpiece/KPIECE1.h"
45 #include "ompl/util/Console.h"
46 #include <memory>
47 #include <algorithm>
48 #include <limits>
49 #include <cmath>
50 #include <map>
51 
53 namespace ompl
54 {
55  namespace tools
56  {
57  class SelfConfig::SelfConfigImpl
58  {
59  friend class SelfConfig;
60 
61  public:
62  SelfConfigImpl(const base::SpaceInformationPtr &si)
63  : wsi_(si), probabilityOfValidState_(-1.0), averageValidMotionLength_(-1.0)
64  {
65  }
66 
68  {
69  base::SpaceInformationPtr si = wsi_.lock();
70  checkSetup(si);
71  if (si && probabilityOfValidState_ < 0.0)
72  probabilityOfValidState_ = si->probabilityOfValidState(magic::TEST_STATE_COUNT);
73  return probabilityOfValidState_;
74  }
75 
77  {
78  base::SpaceInformationPtr si = wsi_.lock();
79  checkSetup(si);
80  if (si && averageValidMotionLength_ < 0.0)
81  averageValidMotionLength_ = si->averageValidMotionLength(magic::TEST_STATE_COUNT);
82  return averageValidMotionLength_;
83  }
84 
85  void configureValidStateSamplingAttempts(unsigned int &attempts)
86  {
87  if (attempts == 0)
89  }
90 
91  void configurePlannerRange(double &range, const std::string &context)
92  {
93  if (range < std::numeric_limits<double>::epsilon())
94  {
95  base::SpaceInformationPtr si = wsi_.lock();
96  if (si)
97  {
98  range = si->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
99  OMPL_DEBUG("%sPlanner range detected to be %lf", context.c_str(), range);
100  }
101  else
102  OMPL_ERROR("%sUnable to detect planner range. SpaceInformation instance has expired.",
103  context.c_str());
104  }
105  }
106 
107  void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj, const std::string &context)
108  {
109  base::SpaceInformationPtr si = wsi_.lock();
110  checkSetup(si);
111  if (!proj && si)
112  {
113  OMPL_INFORM("%sAttempting to use default projection.", context.c_str());
114  proj = si->getStateSpace()->getDefaultProjection();
115  }
116  if (!proj)
117  throw Exception("No projection evaluator specified");
118  proj->setup();
119  }
120 
121  void print(std::ostream &out) const
122  {
123  base::SpaceInformationPtr si = wsi_.lock();
124  if (si)
125  {
126  out << "Configuration parameters for space '" << si->getStateSpace()->getName() << "'" << std::endl;
127  out << " - probability of a valid state is " << probabilityOfValidState_ << std::endl;
128  out << " - average length of a valid motion is " << averageValidMotionLength_ << std::endl;
129  }
130  else
131  out << "EXPIRED" << std::endl;
132  }
133 
134  bool expired() const
135  {
136  return wsi_.expired();
137  }
138 
139  private:
140  void checkSetup(const base::SpaceInformationPtr &si)
141  {
142  if (si)
143  {
144  if (!si->isSetup())
145  {
146  si->setup();
147  probabilityOfValidState_ = -1.0;
148  averageValidMotionLength_ = -1.0;
149  }
150  }
151  else
152  {
153  probabilityOfValidState_ = -1.0;
154  averageValidMotionLength_ = -1.0;
155  }
156  }
157 
158  // we store weak pointers so that the SpaceInformation instances are not kept in
159  // memory until termination of the program due to the use of a static ConfigMap below
160  std::weak_ptr<base::SpaceInformation> wsi_;
161 
162  double probabilityOfValidState_;
163  double averageValidMotionLength_;
164 
165  std::mutex lock_;
166  };
167  }
168 }
169 
170 std::mutex ompl::tools::SelfConfig::staticConstructorLock_;
172 
173 ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context)
174  : context_(context.empty() ? "" : context + ": ")
175 {
176  using ConfigMap = std::map<base::SpaceInformation *, std::shared_ptr<SelfConfigImpl>>;
177 
178  std::unique_lock<std::mutex> smLock(staticConstructorLock_);
179 
180  static ConfigMap SMAP;
181 
182  // clean expired entries from the map
183  auto dit = SMAP.begin();
184  while (dit != SMAP.end())
185  {
186  if (dit->second->expired())
187  SMAP.erase(dit++);
188  else
189  ++dit;
190  }
191 
192  const auto it = SMAP.find(si.get());
193 
194  if (it != SMAP.end())
195  impl_ = it->second.get();
196  else
197  {
198  impl_ = new SelfConfigImpl(si);
199  SMAP[si.get()].reset(impl_);
200  }
201 }
202 
203 ompl::tools::SelfConfig::~SelfConfig() = default;
204 
205 /* ------------------------------------------------------------------------ */
206 
208 {
209  std::lock_guard<std::mutex> iLock(impl_->lock_);
210  return impl_->getProbabilityOfValidState();
211 }
212 
214 {
215  std::lock_guard<std::mutex> iLock(impl_->lock_);
216  return impl_->getAverageValidMotionLength();
217 }
218 
220 {
221  std::lock_guard<std::mutex> iLock(impl_->lock_);
222  impl_->configureValidStateSamplingAttempts(attempts);
223 }
224 
226 {
227  std::lock_guard<std::mutex> iLock(impl_->lock_);
228  impl_->configurePlannerRange(range, context_);
229 }
230 
231 void ompl::tools::SelfConfig::configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
232 {
233  std::lock_guard<std::mutex> iLock(impl_->lock_);
234  return impl_->configureProjectionEvaluator(proj, context_);
235 }
236 
237 void ompl::tools::SelfConfig::print(std::ostream &out) const
238 {
239  std::lock_guard<std::mutex> iLock(impl_->lock_);
240  impl_->print(out);
241 }
242 
244 {
245  base::PlannerPtr planner;
246  base::SpaceInformationPtr si(goal->getSpaceInformation());
247  const base::StateSpacePtr &space(si->getStateSpace());
248  control::SpaceInformationPtr siC(std::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si));
249  if (siC) // kinodynamic planning
250  {
251  // if we have a default projection
252  if (space->hasDefaultProjection())
253  planner = std::make_shared<control::KPIECE1>(siC);
254  // otherwise use a single-tree planner
255  else
256  planner = std::make_shared<control::RRT>(siC);
257  }
258  // if we can sample the goal region and interpolation between states is symmetric,
259  // use a bi-directional planner
260  else if (!goal)
261  {
262  OMPL_WARN("No goal specified; choosing RRT as the default planner");
263  planner = std::make_shared<geometric::RRT>(goal->getSpaceInformation());
264  }
265  else if (goal->hasType(base::GOAL_SAMPLEABLE_REGION) && space->hasSymmetricInterpolate())
266  {
267  // if we have a default projection
268  if (space->hasDefaultProjection())
269  planner = std::make_shared<geometric::LBKPIECE1>(goal->getSpaceInformation());
270  else
271  planner = std::make_shared<geometric::RRTConnect>(goal->getSpaceInformation());
272  }
273  // otherwise use a single-tree planner
274  else
275  {
276  // if we have a default projection
277  if (space->hasDefaultProjection())
278  planner = std::make_shared<geometric::KPIECE1>(goal->getSpaceInformation());
279  else
280  planner = std::make_shared<geometric::RRT>(goal->getSpaceInformation());
281  }
282 
283  if (!planner)
284  throw Exception("Unable to allocate default planner");
285 
286  return planner;
287 }
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void configureValidStateSamplingAttempts(unsigned int &attempts)
Instances of base::ValidStateSampler need a number of attempts to be specified – the maximum number o...
Definition: SelfConfig.cpp:219
static const double MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION
For planners: if default values are to be used for the maximum length of motions, this constant defin...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
A shared pointer wrapper for ompl::base::Planner.
SelfConfig(const base::SpaceInformationPtr &si, const std::string &context=std::string())
Construct an instance that can configure the space encapsulated by si. Any information printed to the...
Definition: SelfConfig.cpp:173
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:231
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
double getProbabilityOfValidState()
Get the probability of a sampled state being valid (calls base::SpaceInformation::probabilityOfValidS...
Definition: SelfConfig.cpp:207
void print(std::ostream &out=std::cout) const
Print the computed configuration parameters.
Definition: SelfConfig.cpp:237
The exception type for ompl.
Definition: Exception.h:78
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:243
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
double getAverageValidMotionLength()
Get the probability of a sampled state being valid (calls base::SpaceInformation::averageValidMotionL...
Definition: SelfConfig.cpp:213
Main namespace. Contains everything in this library.