13 _si = _simple_setup.getSpaceInformation();
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)
33 ob::RealVectorBounds bounds(_planningDimension);
35 if(!planning_bounds_z.empty() && _planningDimension ==3)
37 bounds.setLow(2, planning_bounds_z[0]);
38 bounds.setHigh(2, planning_bounds_z[1]);
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]);
46 _space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
51 _depth_limit = depth_limit;
52 if(_checker !=
nullptr)
58 ob::PlannerPtr planner;
59 if(plannerName.compare(
"CForest")==0)
60 planner = ob::PlannerPtr(
new og::CForest(_si));
62 planner = ob::PlannerPtr(
new og::RRTstar(_si));
68 _simple_setup.setPlanner(planner);
73 if(state_checker !=
nullptr)
74 _simple_setup.setStateValidityChecker(state_checker);
76 cout<<
"ERROR : invalid pointer on state_checker\n";
81 if(_checker !=
nullptr)
89 _checker->
setCollisionMask(collision_mask_x,collision_mask_y,collision_mask_z);
91 _simple_setup.setStateValidityChecker(ob::StateValidityCheckerPtr(_checker));
96 if(_checker !=
nullptr)
104 _checker->
setCollisionMask(collision_mask_x,collision_mask_y,collision_mask_z);
106 _simple_setup.setStateValidityChecker(ob::StateValidityCheckerPtr(_checker));
111 if(_checker !=
nullptr)
118 ob::ScopedState<> start(_space);
119 ob::ScopedState<> goal(_space);
120 start[0] = double(start_state[0]);
121 start[1] = double(start_state[1]);
123 goal[0] = double(goal_state[0]);
124 goal[1] = double(goal_state[1]);
126 if(_planningDimension==3)
128 start[2] = double(start_state[2]);
129 goal[2] = double(goal_state[2]);
132 if (start[2] < _depth_limit) start[2] = _depth_limit;
133 if (goal[2] < _depth_limit) goal[2] = _depth_limit;
136 _simple_setup.setStartAndGoalStates(start, goal);
142 _simple_setup.clear();
144 _simple_setup.setup();
148 ob::PlannerStatus solved = _simple_setup.solve( solving_time );
150 if (solved && _simple_setup.haveExactSolutionPath())
160 if(_checker !=
nullptr)
162 ob::ScopedState<> pos(_space);
164 if(_planningDimension==2)
166 pos[0] = double(position[0]);
167 pos[1] = double(position[1]);
168 return _checker->
isValid(pos->as<ob::SE2StateSpace::StateType>());
172 pos[0] = double(position[0]);
173 pos[1] = double(position[1]);
176 if(position[2]<_depth_limit)
179 pos[2] = double(position[2]);
180 return _checker->
isValid(pos->as<ob::SE3StateSpace::StateType>());
189 og::PathGeometric path =
getPath();
194 bool OMPL_Planner::updatePath(octomap::OcTree* octree,
double solving_time,
const std::vector<double> start_state,
const std::vector<double> goal_state)
209 return _simple_setup.getSolutionPath();
214 return _simple_setup;
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.