CForestStateSampler.cpp
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 /* Author: Javier V. Gómez*/
36 
37 #include "ompl/geometric/planners/cforest/CForestStateSampler.h"
38 
40 {
41  if (!statesToSample_.empty())
42  getNextSample(state);
43  else
44  sampler_->sampleUniform(state);
45 }
46 
47 void ompl::base::CForestStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
48 {
49  if (!statesToSample_.empty())
50  getNextSample(state);
51  else
52  sampler_->sampleUniformNear(state, near, distance);
53 }
54 
55 void ompl::base::CForestStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
56 {
57  if (!statesToSample_.empty())
58  getNextSample(state);
59  else
60  sampler_->sampleGaussian(state, mean, stdDev);
61 }
62 
63 void ompl::base::CForestStateSampler::setStatesToSample(const std::vector<const State *> &states)
64 {
65  std::lock_guard<std::mutex> slock(statesLock_);
66  for (auto &i : statesToSample_)
67  space_->freeState(i);
68  statesToSample_.clear();
69 
70  statesToSample_.reserve(states.size());
71  // push in reverse order, so that the states are popped in order in getNextSample()
72  for (auto state : states)
73  {
74  State *s = space_->allocState();
75  space_->copyState(s, state);
76  statesToSample_.push_back(s);
77  }
78 }
79 
81 {
82  std::lock_guard<std::mutex> slock(statesLock_);
83  space_->copyState(state, statesToSample_.back());
85  statesToSample_.pop_back();
86 }
87 
88 void ompl::base::CForestStateSampler::clear()
89 {
90  std::lock_guard<std::mutex> slock(statesLock_);
91  for (auto &i : statesToSample_)
92  space_->freeState(i);
93  statesToSample_.clear();
94  sampler_.reset();
95 }
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:104
StateSamplerPtr sampler_
Underlying, user-specified state sampler.
std::vector< State * > statesToSample_
States to be sampled.
std::mutex statesLock_
Lock to control the access to the statesToSample_ vector.
void sampleUniformNear(State *state, const State *near, double distance) override
It will sample the next state of the vector StatesToSample_. If this is empty, it will call the sampl...
Definition of an abstract state.
Definition: State.h:49
void setStatesToSample(const std::vector< const State *> &states)
Fills the vector StatesToSample_ of states to be sampled in the next calls to sampleUniform(), sampleUniformNear() or sampleGaussian().
void sampleUniform(State *state) override
It will sample the next state of the vector StatesToSample_. If this is empty, it will call the sampl...
void getNextSample(State *state)
Extracts the next sample when statesToSample_ is not empty.
void sampleGaussian(State *state, const State *mean, double stdDev) override
It will sample the next state of the vector StatesToSample_. If this is empty, it will call the sampl...
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.