All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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/util/Console.h"
40 #include <boost/thread/mutex.hpp>
41 #include <boost/shared_ptr.hpp>
42 #include <boost/weak_ptr.hpp>
43 #include <algorithm>
44 #include <limits>
45 #include <cmath>
46 #include <map>
47 
49 namespace ompl
50 {
51  namespace tools
52  {
53 
54  class SelfConfig::SelfConfigImpl
55  {
56  friend class SelfConfig;
57 
58  public:
59 
60  SelfConfigImpl(const base::SpaceInformationPtr &si) :
61  wsi_(si), probabilityOfValidState_(-1.0), averageValidMotionLength_(-1.0)
62  {
63  }
64 
65  double getProbabilityOfValidState(void)
66  {
67  base::SpaceInformationPtr si = wsi_.lock();
68  checkSetup(si);
69  if (si && probabilityOfValidState_ < 0.0)
70  probabilityOfValidState_ = si->probabilityOfValidState(magic::TEST_STATE_COUNT);
71  return probabilityOfValidState_;
72  }
73 
74  double getAverageValidMotionLength(void)
75  {
76  base::SpaceInformationPtr si = wsi_.lock();
77  checkSetup(si);
78  if (si && averageValidMotionLength_ < 0.0)
79  averageValidMotionLength_ = si->averageValidMotionLength(magic::TEST_STATE_COUNT);
80  return averageValidMotionLength_;
81  }
82 
83  void configureValidStateSamplingAttempts(unsigned int &attempts)
84  {
85  if (attempts == 0)
87  }
88 
89  void configurePlannerRange(double &range, const std::string &context)
90  {
91  if (range < std::numeric_limits<double>::epsilon())
92  {
93  base::SpaceInformationPtr si = wsi_.lock();
94  if (si)
95  {
96  range = si->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
97  OMPL_DEBUG("%sPlanner range detected to be %lf", context.c_str(), range);
98  }
99  else
100  OMPL_ERROR("%sUnable to detect planner range. SpaceInformation instance has expired.", context.c_str());
101  }
102  }
103 
104  void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj, const std::string &context)
105  {
106  base::SpaceInformationPtr si = wsi_.lock();
107  checkSetup(si);
108  if (!proj && si)
109  {
110  OMPL_INFORM("%sAttempting to use default projection.", context.c_str());
111  proj = si->getStateSpace()->getDefaultProjection();
112  }
113  if (!proj)
114  throw Exception("No projection evaluator specified");
115  proj->setup();
116  }
117 
118  void print(std::ostream &out) const
119  {
120  base::SpaceInformationPtr si = wsi_.lock();
121  if (si)
122  {
123  out << "Configuration parameters for space '" << si->getStateSpace()->getName() << "'" << std::endl;
124  out << " - probability of a valid state is " << probabilityOfValidState_ << std::endl;
125  out << " - average length of a valid motion is " << averageValidMotionLength_ << std::endl;
126  }
127  else
128  out << "EXPIRED" << std::endl;
129  }
130 
131  bool expired(void) const
132  {
133  return wsi_.expired();
134  }
135 
136  private:
137 
138  void checkSetup(const base::SpaceInformationPtr &si)
139  {
140  if (si)
141  {
142  if (!si->isSetup())
143  {
144  si->setup();
145  probabilityOfValidState_ = -1.0;
146  averageValidMotionLength_ = -1.0;
147  }
148  }
149  else
150  {
151  probabilityOfValidState_ = -1.0;
152  averageValidMotionLength_ = -1.0;
153  }
154  }
155 
156  // we store weak pointers so that the SpaceInformation instances are not kept in
157  // memory until termination of the program due to the use of a static ConfigMap below
158  boost::weak_ptr<base::SpaceInformation> wsi_;
159 
160  double probabilityOfValidState_;
161  double averageValidMotionLength_;
162 
163  boost::mutex lock_;
164  };
165 
166  }
167 }
168 
170 
171 ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context) :
172  context_(context.empty() ? "" : context + ": ")
173 {
174  typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap;
175 
176  static ConfigMap SMAP;
177  static boost::mutex LOCK;
178 
179  boost::mutex::scoped_lock smLock(LOCK);
180 
181  // clean expired entries from the map
182  ConfigMap::iterator dit = SMAP.begin();
183  while (dit != SMAP.end())
184  {
185  if (dit->second->expired())
186  SMAP.erase(dit++);
187  else
188  ++dit;
189  }
190 
191  ConfigMap::const_iterator it = SMAP.find(si.get());
192 
193  if (it != SMAP.end())
194  impl_ = it->second.get();
195  else
196  {
197  impl_ = new SelfConfigImpl(si);
198  SMAP[si.get()].reset(impl_);
199  }
200 }
201 
202 ompl::tools::SelfConfig::~SelfConfig(void)
203 {
204 }
205 
206 /* ------------------------------------------------------------------------ */
207 
209 {
210  boost::mutex::scoped_lock iLock(impl_->lock_);
211  return impl_->getProbabilityOfValidState();
212 }
213 
215 {
216  boost::mutex::scoped_lock iLock(impl_->lock_);
217  return impl_->getAverageValidMotionLength();
218 }
219 
221 {
222  boost::mutex::scoped_lock iLock(impl_->lock_);
223  impl_->configureValidStateSamplingAttempts(attempts);
224 }
225 
227 {
228  boost::mutex::scoped_lock iLock(impl_->lock_);
229  impl_->configurePlannerRange(range, context_);
230 }
231 
233 {
234  boost::mutex::scoped_lock iLock(impl_->lock_);
235  return impl_->configureProjectionEvaluator(proj, context_);
236 }
237 
238 void ompl::tools::SelfConfig::print(std::ostream &out) const
239 {
240  boost::mutex::scoped_lock iLock(impl_->lock_);
241  impl_->print(out);
242 }
void print(std::ostream &out=std::cout) const
Print the computed configuration parameters.
Definition: SelfConfig.cpp:238
double getAverageValidMotionLength(void)
Get the probability of a sampled state being valid (calls base::SpaceInformation::averageValidMotionL...
Definition: SelfConfig.cpp:214
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
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:171
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:232
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:226
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
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...
void configureValidStateSamplingAttempts(unsigned int &attempts)
Instances of base::ValidStateSampler need a number of attempts to be specified – the maximum number ...
Definition: SelfConfig.cpp:220
double getProbabilityOfValidState(void)
Get the probability of a sampled state being valid (calls base::SpaceInformation::probabilityOfValidS...
Definition: SelfConfig.cpp:208
static const unsigned int MAX_VALID_SAMPLE_ATTEMPTS
When multiple attempts are needed to generate valid samples, this value defines the default number of...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68