All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
GoalLazySamples.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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/base/goals/GoalLazySamples.h"
38 #include "ompl/base/ScopedState.h"
39 #include "ompl/util/Time.h"
40 
41 ompl::base::GoalLazySamples::GoalLazySamples(const SpaceInformationPtr &si, const GoalSamplingFn &samplerFunc, bool autoStart, double minDist) :
42  GoalStates(si), samplerFunc_(samplerFunc), terminateSamplingThread_(false), samplingThread_(NULL), samplingAttempts_(0), minDist_(minDist)
43 {
45  if (autoStart)
46  startSampling();
47 }
48 
49 ompl::base::GoalLazySamples::~GoalLazySamples(void)
50 {
51  stopSampling();
52 }
53 
55 {
56  if (samplingThread_ == NULL)
57  {
58  OMPL_DEBUG("Starting goal sampling thread");
59  terminateSamplingThread_ = false;
60  samplingThread_ = new boost::thread(&GoalLazySamples::goalSamplingThread, this);
61  }
62 }
63 
65 {
66  if (isSampling())
67  {
68  OMPL_DEBUG("Attempting to stop goal sampling thread...");
69  terminateSamplingThread_ = true;
70  samplingThread_->join();
71  delete samplingThread_;
72  samplingThread_ = NULL;
73  }
74  else
75  if (samplingThread_)
76  { // join a finished thread
77  samplingThread_->join();
78  delete samplingThread_;
79  samplingThread_ = NULL;
80  }
81 }
82 
84 {
85  if (!si_->isSetup())
86  {
87  OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
88  // wait for everything to be set up before performing computation
89  while (!terminateSamplingThread_ && !si_->isSetup())
90  boost::this_thread::sleep(time::seconds(0.01));
91  }
92  unsigned int prevsa = samplingAttempts_;
93  if (!terminateSamplingThread_ && samplerFunc_)
94  {
95  OMPL_DEBUG("Beginning sampling thread computation");
96  ScopedState<> s(si_);
97  while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
98  {
99  ++samplingAttempts_;
100  if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
101  addStateIfDifferent(s.get(), minDist_);
102  }
103  }
104  else
105  OMPL_WARN("Goal sampling thread never did any work.%s",
106  samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function set.");
107  terminateSamplingThread_ = true;
108  OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
109 }
110 
112 {
113  return terminateSamplingThread_ == false && samplingThread_ != NULL;
114 }
115 
117 {
118  return canSample() || isSampling();
119 }
120 
122 {
123  boost::mutex::scoped_lock slock(lock_);
125 }
126 
128 {
129  boost::mutex::scoped_lock slock(lock_);
130  return GoalStates::distanceGoal(st);
131 }
132 
134 {
135  boost::mutex::scoped_lock slock(lock_);
137 }
138 
140 {
141  callback_ = callback;
142 }
143 
145 {
146  boost::mutex::scoped_lock slock(lock_);
148 }
149 
151 {
152  boost::mutex::scoped_lock slock(lock_);
153  return GoalStates::getState(index);
154 }
155 
157 {
158  boost::mutex::scoped_lock slock(lock_);
159  return GoalStates::hasStates();
160 }
161 
163 {
164  boost::mutex::scoped_lock slock(lock_);
165  return GoalStates::getStateCount();
166 }
167 
168 bool ompl::base::GoalLazySamples::addStateIfDifferent(const State* st, double minDistance)
169 {
170  const base::State *newState = NULL;
171  bool added = false;
172  {
173  boost::mutex::scoped_lock slock(lock_);
174  if (GoalStates::distanceGoal(st) > minDistance)
175  {
177  added = true;
178  if (callback_)
179  newState = states_.back();
180  }
181  }
182 
183  // the lock is released at this; if needed, issue a call to the callback
184  if (newState)
185  callback_(newState);
186  return added;
187 }
bool isSampling(void) const
Return true if the sampling thread is active.
void startSampling(void)
Start the goal sampling thread.
virtual bool hasStates(void) const
Check if there are any states in this goal region.
Definition: GoalStates.cpp:118
void stopSampling(void)
Stop the goal sampling thread.
void goalSamplingThread(void)
The function that samples goals by calling samplerFunc_ in a separate thread.
virtual void clear(void)
Clear all goal states.
Definition of a scoped state.
Definition: ScopedState.h:56
virtual double distanceGoal(const State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
Definition: GoalStates.cpp:60
boost::function< void(const base::State *)> NewStateCallbackFn
When new samples are generated and added to the list of possible samples, a callback can be called...
virtual void sampleGoal(State *st) const
Sample a state in the goal region.
virtual std::size_t getStateCount(void) const
Return the number of valid goal states.
Definition: GoalStates.cpp:113
virtual bool couldSample(void) const
Return true if GoalStates::couldSample() is true or if the sampling thread is active, as in this case it is possible a sample can be produced at some point.
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition of a set of goal states.
Definition: GoalStates.h:50
virtual std::size_t getStateCount(void) const
Return the number of valid goal states.
bool addStateIfDifferent(const State *st, double minDistance)
Add a state st if it further away that minDistance from previously added states. Return true if the s...
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
Definition: GoalStates.cpp:105
virtual void addState(const State *st)
Add a goal state.
Definition: GoalStates.cpp:95
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
virtual void sampleGoal(State *st) const
Sample a state in the goal region.
Definition: GoalStates.cpp:82
GoalType type_
Goal type.
Definition: Goal.h:147
void setNewStateCallback(const NewStateCallbackFn &callback)
Set the callback function to be called when a new state is added to the list of possible samples...
StateType * get(void)
Returns a pointer to the contained state.
Definition: ScopedState.h:396
GoalLazySamples(const SpaceInformationPtr &si, const GoalSamplingFn &samplerFunc, bool autoStart=true, double minDist=std::numeric_limits< double >::epsilon())
Create a goal region that can be sampled in a lazy fashion. A function (samplerFunc) that produces sa...
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
virtual void addState(const State *st)
Add a goal state.
virtual bool hasStates(void) const
Check if there are any states in this goal region.
virtual double distanceGoal(const State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
virtual void clear(void)
Clear all goal states.
Definition: GoalStates.cpp:48
boost::function< bool(const GoalLazySamples *, State *)> GoalSamplingFn
Goal sampling function. Returns false when no further calls should be made to it. Fills its second ar...
This bit is set if casting to goal states (ompl::base::GoalLazySamples) is possible.
Definition: GoalTypes.h:64