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 #include <ompl/base/SpaceInformation.h>
00038 #include <ompl/base/spaces/SE3StateSpace.h>
00039 #include <ompl/base/samplers/ObstacleBasedValidStateSampler.h>
00040 #include <ompl/geometric/planners/rrt/RRTConnect.h>
00041 #include <ompl/geometric/SimpleSetup.h>
00042
00043 #include <ompl/config.h>
00044 #include <boost/thread.hpp>
00045 #include <iostream>
00046
00047 namespace ob = ompl::base;
00048 namespace og = ompl::geometric;
00049
00051
00052
00053
00054
00055
00056
00057
00058
00059 class MyValidStateSampler : public ob::ValidStateSampler
00060 {
00061 public:
00062 MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
00063 {
00064 name_ = "my sampler";
00065 }
00066
00067
00068
00069
00070 virtual bool sample(ob::State *state)
00071 {
00072 double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;
00073 double z = rng_.uniformReal(-1,1);
00074
00075 if (z>.25 && z<.5)
00076 {
00077 double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
00078 switch(rng_.uniformInt(0,3))
00079 {
00080 case 0: val[0]=x-1; val[1]=y-1;
00081 case 1: val[0]=x-.8; val[1]=y+.8;
00082 case 2: val[0]=y-1; val[1]=x-1;
00083 case 3: val[0]=y+.8; val[1]=x-.8;
00084 }
00085 }
00086 else
00087 {
00088 val[0] = rng_.uniformReal(-1,1);
00089 val[1] = rng_.uniformReal(-1,1);
00090 }
00091 val[2] = z;
00092 assert(si_->isValid(state));
00093 return true;
00094 }
00095
00096 virtual bool sampleNear(ob::State *state, const ob::State *near, const double distance)
00097 {
00098 throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
00099 return false;
00100 }
00101 protected:
00102 ompl::RNG rng_;
00103 };
00104
00106
00107
00108
00109 bool isStateValid(const ob::State *state)
00110 {
00111 const ob::RealVectorStateSpace::StateType& pos = *state->as<ob::RealVectorStateSpace::StateType>();
00112
00113
00114
00115 boost::this_thread::sleep(ompl::time::seconds(.001));
00116
00117
00118
00119 return !(fabs(pos[0]<.8) && fabs(pos[1]<.8) && pos[2]>.25 && pos[2]<.5);
00120 }
00121
00122
00123 ob::ValidStateSamplerPtr allocOBValidStateSampler(const ob::SpaceInformation *si)
00124 {
00125
00126
00127 return ob::ValidStateSamplerPtr(new ob::ObstacleBasedValidStateSampler(si));
00128 }
00129
00130
00131 ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
00132 {
00133 return ob::ValidStateSamplerPtr(new MyValidStateSampler(si));
00134 }
00135
00136
00137 void plan(int samplerIndex)
00138 {
00139
00140 ob::StateSpacePtr space(new ob::RealVectorStateSpace(3));
00141
00142
00143 ob::RealVectorBounds bounds(3);
00144 bounds.setLow(-1);
00145 bounds.setHigh(1);
00146 space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
00147
00148
00149 og::SimpleSetup ss(space);
00150
00151
00152 ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
00153
00154
00155 ob::ScopedState<> start(space);
00156 start[0] = start[1] = start[2] = 0;
00157
00158
00159 ob::ScopedState<> goal(space);
00160 goal[0] = goal[1] = 0.;
00161 goal[2] = 1;
00162
00163
00164 ss.setStartAndGoalStates(start, goal);
00165
00166
00167 if (samplerIndex==1)
00168
00169 ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
00170 else if (samplerIndex==2)
00171
00172 ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);
00173
00174
00175
00176 ob::PlannerPtr planner(new og::RRTConnect(ss.getSpaceInformation()));
00177 ss.setPlanner(planner);
00178
00179
00180 bool solved = ss.solve(10.0);
00181 if (solved)
00182 {
00183 std::cout << "Found solution:" << std::endl;
00184
00185 ss.simplifySolution();
00186 ss.getSolutionPath().print(std::cout);
00187 }
00188 else
00189 std::cout << "No solution found" << std::endl;
00190 }
00191
00192 int main(int, char **)
00193 {
00194 std::cout << "Using default uniform sampler:" << std::endl;
00195 plan(0);
00196 std::cout << "\nUsing obstacle-based sampler:" << std::endl;
00197 plan(1);
00198 std::cout << "\nUsing my sampler:" << std::endl;
00199 plan(2);
00200
00201 return 0;
00202 }