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_GEOMETRIC_PLANNERS_KPIECE_LBKPIECE1_
00038 #define OMPL_GEOMETRIC_PLANNERS_KPIECE_LBKPIECE1_
00039
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/geometric/planners/kpiece/Discretization.h"
00042
00043 namespace ompl
00044 {
00045
00046 namespace geometric
00047 {
00048
00078 class LBKPIECE1 : public base::Planner
00079 {
00080 public:
00081
00083 LBKPIECE1(const base::SpaceInformationPtr &si);
00084
00085 virtual ~LBKPIECE1(void);
00086
00089 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00090 {
00091 projectionEvaluator_ = projectionEvaluator;
00092 }
00093
00096 void setProjectionEvaluator(const std::string &name)
00097 {
00098 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00099 }
00100
00102 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00103 {
00104 return projectionEvaluator_;
00105 }
00106
00112 void setRange(double distance)
00113 {
00114 maxDistance_ = distance;
00115 }
00116
00118 double getRange(void) const
00119 {
00120 return maxDistance_;
00121 }
00128 void setBorderFraction(double bp)
00129 {
00130 dStart_.setBorderFraction(bp);
00131 dGoal_.setBorderFraction(bp);
00132 }
00133
00136 double getBorderFraction(void) const
00137 {
00138 return dStart_.getBorderFraction();
00139 }
00140
00147 void setMinValidPathFraction(double fraction)
00148 {
00149 minValidPathFraction_ = fraction;
00150 }
00151
00153 double getMinValidPathFraction(void) const
00154 {
00155 return minValidPathFraction_;
00156 }
00157
00158 virtual void setup(void);
00159
00160 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00161 virtual void clear(void);
00162
00163 virtual void getPlannerData(base::PlannerData &data) const;
00164
00165 protected:
00166
00168 class Motion
00169 {
00170 public:
00171
00172 Motion(void) : root(NULL), state(NULL), parent(NULL), valid(false)
00173 {
00174 }
00175
00177 Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
00178 {
00179 }
00180
00181 ~Motion(void)
00182 {
00183 }
00184
00186 const base::State *root;
00187
00189 base::State *state;
00190
00192 Motion *parent;
00193
00195 bool valid;
00196
00198 std::vector<Motion*> children;
00199 };
00200
00202 void freeMotion(Motion *motion);
00203
00205 void removeMotion(Discretization<Motion> &disc, Motion* motion);
00206
00212 bool isPathValid(Discretization<Motion> &disc, Motion* motion, base::State *temp);
00213
00215 base::StateSamplerPtr sampler_;
00216
00218 base::ProjectionEvaluatorPtr projectionEvaluator_;
00219
00221 Discretization<Motion> dStart_;
00222
00224 Discretization<Motion> dGoal_;
00225
00231 double minValidPathFraction_;
00232
00234 double maxDistance_;
00235
00237 RNG rng_;
00238
00240 std::pair<base::State*, base::State*> connectionPoint_;
00241 };
00242
00243 }
00244 }
00245
00246
00247 #endif