MinimumClearanceValidStateSampler.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, University of Colorado, Boulder
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 Univ of CO, Boulder 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: Dave Coleman <dave@dav.ee>
36  Desc: Find a valid sample with a minimum distance to nearby obstacles
37  (clearance threshold)
38 */
39 
40 #include "ompl/base/samplers/MinimumClearanceValidStateSampler.h"
41 #include "ompl/base/SpaceInformation.h"
42 
44  : ValidStateSampler(si), sampler_(si->allocStateSampler()), clearance_(1)
45 {
46  name_ = "min_clearance";
47  params_.declareParam<double>("min_obstacle_clearance",
48  [this](double c)
49  {
51  },
52  [this]
53  {
55  });
56 }
57 
59 {
60  unsigned int attempts = 0;
61  bool valid = false;
62  double dist = 0.0;
63  do
64  {
65  sampler_->sampleUniform(state);
66  valid = si_->getStateValidityChecker()->isValid(state, dist);
67 
68  // Also check for distance to nearest obstacle and invalidate if too close
69  if (dist < clearance_)
70  {
71  valid = false;
72  }
73 
74  ++attempts;
75  } while (!valid && attempts < attempts_);
76 
77  return valid;
78 }
79 
80 bool ompl::base::MinimumClearanceValidStateSampler::sampleNear(State *state, const State *near, const double distance)
81 {
82  unsigned int attempts = 0;
83  bool valid = false;
84  double dist = 0.0;
85  do
86  {
87  sampler_->sampleUniformNear(state, near, distance);
88  valid = si_->getStateValidityChecker()->isValid(state, dist);
89 
90  // Also check for distance to nearest obstacle and invalidate if too close
91  if (dist < clearance_)
92  {
93  valid = false;
94  }
95 
96  ++attempts;
97  } while (!valid && attempts < attempts_);
98 
99  return valid;
100 }
const SpaceInformation * si_
The state space this sampler samples.
double clearance_
Minimum required distance of sample from nearest obstacle to be considered valid. ...
const StateValidityCheckerPtr & getStateValidityChecker() const
Return the instance of the used state validity checker.
Abstract definition of a state sampler.
The base class for space information. This contains all the information about the space planning is d...
bool sample(State *state) override
Sample a state. Return false in case of failure.
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
unsigned int attempts_
Number of attempts to find a valid sample.
Definition of an abstract state.
Definition: State.h:49
MinimumClearanceValidStateSampler(const SpaceInformation *si)
Constructor.
void setMinimumObstacleClearance(double clearance)
Set the minimum required distance of sample from nearest obstacle to be considered valid...
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=typename SpecificParam< T >::GetterFn())
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:232
double getMinimumObstacleClearance() const
Get the minimum required distance of sample from nearest obstacle to be considered valid...
ParamSet params_
The parameters for this instance of the valid state sampler.
StateSamplerPtr sampler_
The sampler to build upon.
std::string name_
The name of the sampler.