OMPL Plannification
Path planning with OMPL
OMPL_Planner.cpp
Go to the documentation of this file.
1 
9 #include "OMPL_Planner.hpp"
10 
11 OMPL_Planner::OMPL_Planner(int dim) : _planningDimension(dim), _space(new ob::RealVectorStateSpace(dim)), _simple_setup(_space)
12 {
13  _si = _simple_setup.getSpaceInformation();
14  /*
15  _start_state.resize(3);
16  _goal_state.resize(3);
17  */
18 }
19 
21 {
22  /*
23  if(_checker !=nullptr)
24  {
25  delete _checker;
26  _checker = nullptr;
27  }
28  */
29 }
30 
31 void OMPL_Planner::setPlanningBounds(const std::vector<double> planning_bounds_x, const std::vector<double> planning_bounds_y, const std::vector<double> planning_bounds_z)
32 {
33  ob::RealVectorBounds bounds(_planningDimension);
34 
35  if(!planning_bounds_z.empty() && _planningDimension ==3)
36  {
37  bounds.setLow(2, planning_bounds_z[0]);
38  bounds.setHigh(2, planning_bounds_z[1]);
39  }
40 
41  bounds.setLow(0, planning_bounds_x[0]);
42  bounds.setHigh(0, planning_bounds_x[1]);
43  bounds.setLow(1, planning_bounds_y[0]);
44  bounds.setHigh(1, planning_bounds_y[1]);
45 
46  _space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
47 }
48 
49 void OMPL_Planner::setPlanningDepth(double depth_limit)
50 {
51  _depth_limit = depth_limit;
52  if(_checker !=nullptr)
53  _checker->setDepthLimit(depth_limit);
54 }
55 
56 void OMPL_Planner::setPlanner(const std::string plannerName)
57 {
58  ob::PlannerPtr planner;
59  if(plannerName.compare("CForest")==0)
60  planner = ob::PlannerPtr(new og::CForest(_si)); //Deleted by simple_setup destructor ?
61  else //Default planner
62  planner = ob::PlannerPtr(new og::RRTstar(_si));
63 
64  //planner->as<og::RRTstar>()->setRange(0.2);
65  //=======================================================================
66  // Set the setup planner
67  //=======================================================================
68  _simple_setup.setPlanner(planner);
69 }
70 
71 void OMPL_Planner::setStateChecker(const ob::StateValidityCheckerPtr state_checker)
72 {
73  if(state_checker != nullptr)
74  _simple_setup.setStateValidityChecker(state_checker);
75  else
76  cout<<"ERROR : invalid pointer on state_checker\n";
77 }
78 
79 void OMPL_Planner::setStateChecker(const std::string mapFile, double collision_mask_x, double collision_mask_y, double collision_mask_z)
80 {
81  if(_checker !=nullptr)
82  {
83  delete _checker;
84  _checker = nullptr;
85  }
86 
87  _checker = new StateChecker(_si);
88  _checker->loadMap(mapFile);
89  _checker->setCollisionMask(collision_mask_x,collision_mask_y,collision_mask_z);
90  _checker->setDepthLimit(_depth_limit);
91  _simple_setup.setStateValidityChecker(ob::StateValidityCheckerPtr(_checker));
92 }
93 
94 void OMPL_Planner::setStateChecker(octomap::OcTree* octree, double collision_mask_x, double collision_mask_y, double collision_mask_z)
95 {
96  if(_checker !=nullptr)
97  {
98  delete _checker;
99  _checker = nullptr;
100  }
101 
102  _checker = new StateChecker(_si);
103  _checker->loadMap(octree);
104  _checker->setCollisionMask(collision_mask_x,collision_mask_y,collision_mask_z);
105  _checker->setDepthLimit(_depth_limit);
106  _simple_setup.setStateValidityChecker(ob::StateValidityCheckerPtr(_checker));
107 }
108 
109 void OMPL_Planner::updateStateChecker(octomap::OcTree* octree)
110 {
111  if(_checker !=nullptr)
112  _checker->loadMap(octree);
113  //No error warned
114 }
115 
116 void OMPL_Planner::setStartGoal(const std::vector<double> start_state, const std::vector<double> goal_state)
117 {
118  ob::ScopedState<> start(_space);
119  ob::ScopedState<> goal(_space);
120  start[0] = double(start_state[0]);
121  start[1] = double(start_state[1]);
122 
123  goal[0] = double(goal_state[0]);
124  goal[1] = double(goal_state[1]);
125 
126  if(_planningDimension==3)
127  {
128  start[2] = double(start_state[2]);
129  goal[2] = double(goal_state[2]);
130 
131  //Prevent invalid states because of too low altitude (Octomap issue)
132  if (start[2] < _depth_limit) start[2] = _depth_limit;
133  if (goal[2] < _depth_limit) goal[2] = _depth_limit;
134  }
135 
136  _simple_setup.setStartAndGoalStates(start, goal);
137 }
138 
139 bool OMPL_Planner::findPath(double solving_time)
140 {
141  //Clear all previous planning data (Planner settings, start/goal states not affected)
142  _simple_setup.clear();
143 
144  _simple_setup.setup();
145 
146  //_simple_setup.print(cout);
147 
148  ob::PlannerStatus solved = _simple_setup.solve( solving_time );
149 
150  if (solved && _simple_setup.haveExactSolutionPath())
151  {
152  return true;
153  }
154  else
155  return false;
156 }
157 
158 bool OMPL_Planner::isValid(std::vector<double> position) const
159 {
160  if(_checker !=nullptr)
161  {
162  ob::ScopedState<> pos(_space);
163 
164  if(_planningDimension==2)
165  {
166  pos[0] = double(position[0]);
167  pos[1] = double(position[1]);
168  return _checker->isValid(pos->as<ob::SE2StateSpace::StateType>());
169  }
170  else
171  {
172  pos[0] = double(position[0]);
173  pos[1] = double(position[1]);
174 
175  //Prevent invalid states because of too low altitude (Octomap issue)
176  if(position[2]<_depth_limit)
177  pos[2]=_depth_limit;
178  else
179  pos[2] = double(position[2]);
180  return _checker->isValid(pos->as<ob::SE3StateSpace::StateType>());
181  }
182  }
183  else
184  return false; //No error warned
185 }
186 
188 {
189  og::PathGeometric path = getPath();
190 
191  return path.check();
192 }
193 
194 bool OMPL_Planner::updatePath(octomap::OcTree* octree, double solving_time, const std::vector<double> start_state, const std::vector<double> goal_state)
195 {
196  updateStateChecker(octree);
197 
198  if(! checkPath()) //Invalid path
199  {
200  setStartGoal(start_state,goal_state);
201  return findPath(solving_time);
202  }
203  else
204  return true;
205 }
206 
207 og::PathGeometric OMPL_Planner::getPath() const
208 {
209  return _simple_setup.getSolutionPath();
210 }
211 
212 og::SimpleSetup OMPL_Planner::getSimpleSetup() const
213 {
214  return _simple_setup;
215 }
Class for state checking for 2D and 3D planning. Extension of an abstract class used to implement the...
bool updatePath(octomap::OcTree *octree, double solving_time, const std::vector< double > start_state, const std::vector< double > goal_state)
Update the path.
void setStateChecker(const ob::StateValidityCheckerPtr state_checker)
Set the state checker.
void setStartGoal(const std::vector< double > start_state, const std::vector< double > goal_state)
Set the planning objectives.
bool findPath(double solving_time)
Solve the planning problem.
og::PathGeometric getPath() const
Return the path found by the planner.
void setPlanningDepth(double depth_limit=1)
Set the planning depth.
~OMPL_Planner()
Destructor.
void setPlanningBounds(const std::vector< double > planning_bounds_x, const std::vector< double > planning_bounds_y, const std::vector< double > planning_bounds_z=std::vector< double >())
Set the bounds of the planner workspace.
void setDepthLimit(double depth_limit)
Set the depth limit (constant for 2D checking)
void setCollisionMask(double x, double y, double z=0.2)
Set the area around every state considered for state checking.
virtual bool isValid(const ob::State *state) const
State validator.
Path planning (without differential constraints) using OMPL with Octomap to represent workspace and t...
void loadMap(const std::string mapFile)
Load the octomap where the states are checked.
og::SimpleSetup getSimpleSetup() const
Return the OMPL object containing most of the information of the planning problem.
bool checkPath() const
Check if the path is valid.
OMPL_Planner(int dim)
Constructor.
void setPlanner(const std::string plannerName="RRTstar")
Set the planning algorithm used to solve the problem.
void updateStateChecker(octomap::OcTree *octree)
Update the state checker.
bool isValid(std::vector< double > position) const
Check if position is collision-free.