00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_CONTROL_CONTROL_SAMPLER_
00038 #define OMPL_CONTROL_CONTROL_SAMPLER_
00039
00040 #include "ompl/base/State.h"
00041 #include "ompl/control/Control.h"
00042 #include "ompl/util/RandomNumbers.h"
00043 #include "ompl/util/ClassForward.h"
00044 #include <vector>
00045 #include <boost/function.hpp>
00046
00047 namespace ompl
00048 {
00049 namespace control
00050 {
00051
00052 ClassForward(ControlSpace);
00053
00055 ClassForward(ControlSampler);
00056
00065 class ControlSampler
00066 {
00067 public:
00068
00070 ControlSampler(const ControlSpace *space) : space_(space)
00071 {
00072 }
00073
00074 virtual ~ControlSampler(void)
00075 {
00076 }
00077
00081 virtual void sample(Control *control) = 0;
00082
00091 virtual void sample(Control *control, const base::State * )
00092 {
00093 sample(control);
00094 }
00095
00103 virtual void sampleNext(Control *control, const Control * )
00104 {
00105 sample(control);
00106 }
00107
00119 virtual void sampleNext(Control *control, const Control * , const base::State * )
00120 {
00121 sample(control);
00122 }
00123
00125 virtual unsigned int sampleStepCount(unsigned int minSteps, unsigned int maxSteps);
00126
00127 protected:
00128
00130 const ControlSpace *space_;
00131
00133 RNG rng_;
00134 };
00135
00137 class CompoundControlSampler : public ControlSampler
00138 {
00139 public:
00140
00142 CompoundControlSampler(const ControlSpace* space) : ControlSampler(space)
00143 {
00144 }
00145
00147 virtual ~CompoundControlSampler(void)
00148 {
00149 }
00150
00154 virtual void addSampler(const ControlSamplerPtr &sampler);
00155
00156
00157 virtual void sample(Control *control);
00158 virtual void sample(Control *control, const base::State *state);
00159 virtual void sampleNext(Control *control, const Control *previous);
00160 virtual void sampleNext(Control *control, const Control *previous, const base::State *state);
00161
00162 protected:
00163
00165 std::vector<ControlSamplerPtr> samplers_;
00166
00167 private:
00168
00170 unsigned int samplerCount_;
00171
00172 };
00173
00175 typedef boost::function1<ControlSamplerPtr, const ControlSpace*> ControlSamplerAllocator;
00176 }
00177 }
00178
00179
00180 #endif