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 <algorithm>
43 #include <limits>
44 #include <cmath>
45 #include <map>
46 
48 namespace ompl
49 {
50  namespace tools
51  {
52 
53  class SelfConfig::SelfConfigImpl
54  {
55  friend class SelfConfig;
56 
57  public:
58 
59  SelfConfigImpl(const base::SpaceInformationPtr &si) :
60  si_(si), probabilityOfValidState_(-1.0), averageValidMotionLength_(-1.0)
61  {
62  }
63 
64  double getProbabilityOfValidState(void)
65  {
66  checkSetup();
67  if (probabilityOfValidState_ < 0.0)
68  probabilityOfValidState_ = si_->probabilityOfValidState(magic::TEST_STATE_COUNT);
69  return probabilityOfValidState_;
70  }
71 
72  double getAverageValidMotionLength(void)
73  {
74  checkSetup();
75  if (averageValidMotionLength_ < 0.0)
76  averageValidMotionLength_ = si_->averageValidMotionLength(magic::TEST_STATE_COUNT);
77  return averageValidMotionLength_;
78  }
79 
80  void configureValidStateSamplingAttempts(unsigned int &attempts)
81  {
82  if (attempts == 0)
84 
85  /*
86  static const double log_of_0_9 = -0.105360516;
87  if (attempts == 0)
88  {
89  double p = 1.0 - getProbabilityOfValidState();
90  if (p > 0.0)
91  attempts = std::min((unsigned int)std::max((int)ceil(log_of_0_9 / log(p)), 1),
92  magic::MAX_VALID_SAMPLE_ATTEMPTS);
93  else
94  attempts = 1;
95  msg_.debug("Number of attempts made at sampling a valid state in space %s is computed to be %u",
96  si_->getStateSpace()->getName().c_str(), attempts);
97  }
98  */
99  }
100 
101  void configurePlannerRange(double &range)
102  {
103  if (range < std::numeric_limits<double>::epsilon())
104  {
105  range = si_->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
106  OMPL_DEBUG("Planner range detected to be %lf", range);
107  }
108 
109  /*
110  if (range < std::numeric_limits<double>::epsilon())
111  {
112  range = getAverageValidMotionLength() / 2.0;
113  double b = si_->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
114  if (range < std::numeric_limits<double>::epsilon())
115  range = b;
116  else
117  range = std::min(range, b);
118  msg_.debug("Planner range detected to be %lf", range);
119  }
120  */
121  }
122 
123  void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
124  {
125  checkSetup();
126  if (!proj)
127  {
128  OMPL_INFORM("Attempting to use default projection.");
129  proj = si_->getStateSpace()->getDefaultProjection();
130  }
131  if (!proj)
132  throw Exception("No projection evaluator specified");
133  proj->setup();
134  }
135 
136  void print(std::ostream &out) const
137  {
138  out << "Configuration parameters for space '" << si_->getStateSpace()->getName() << "'" << std::endl;
139  out << " - probability of a valid state is " << probabilityOfValidState_ << std::endl;
140  out << " - average length of a valid motion is " << averageValidMotionLength_ << std::endl;
141  }
142 
143  private:
144 
145  void checkSetup(void)
146  {
147  if (!si_->isSetup())
148  {
149  si_->setup();
150  probabilityOfValidState_ = -1.0;
151  averageValidMotionLength_ = -1.0;
152  }
153  }
154 
155  base::SpaceInformationPtr si_;
156  double probabilityOfValidState_;
157  double averageValidMotionLength_;
158 
159  boost::mutex lock_;
160  };
161 
162  }
163 }
164 
166 
167 ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context) : context_(context)
168 {
169  typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap;
170 
171  static ConfigMap SMAP;
172  static boost::mutex LOCK;
173 
174  boost::mutex::scoped_lock smLock(LOCK);
175  ConfigMap::const_iterator it = SMAP.find(si.get());
176 
177  if (it != SMAP.end())
178  impl_ = it->second.get();
179  else
180  {
181  impl_ = new SelfConfigImpl(si);
182  SMAP[si.get()].reset(impl_);
183  }
184 }
185 
186 /* ------------------------------------------------------------------------ */
187 
189 {
190  boost::mutex::scoped_lock iLock(impl_->lock_);
191  return impl_->getProbabilityOfValidState();
192 }
193 
195 {
196  boost::mutex::scoped_lock iLock(impl_->lock_);
197  return impl_->getAverageValidMotionLength();
198 }
199 
201 {
202  boost::mutex::scoped_lock iLock(impl_->lock_);
203  impl_->configureValidStateSamplingAttempts(attempts);
204 }
205 
207 {
208  boost::mutex::scoped_lock iLock(impl_->lock_);
209  impl_->configurePlannerRange(range);
210 }
211 
213 {
214  boost::mutex::scoped_lock iLock(impl_->lock_);
215  return impl_->configureProjectionEvaluator(proj);
216 }
217 
218 void ompl::tools::SelfConfig::print(std::ostream &out) const
219 {
220  boost::mutex::scoped_lock iLock(impl_->lock_);
221  impl_->print(out);
222 }