17 #include <ompl/geometric/SimpleSetup.h> 19 #include <ompl/geometric/planners/cforest/CForest.h> 20 #include <ompl/geometric/planners/rrt/RRTstar.h> 22 #include <ompl/base/spaces/SE3StateSpace.h> 23 #include <ompl/base/spaces/SE2StateSpace.h> 26 namespace ob = ompl::base;
27 namespace og = ompl::geometric;
29 #include "StateChecker.hpp" 51 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>());
66 void setPlanner(
const std::string plannerName =
"RRTstar");
73 void setStateChecker(
const std::string mapFile,
double collision_mask_x,
double collision_mask_y,
double collision_mask_z = 0.2);
74 void setStateChecker(octomap::OcTree* octree,
double collision_mask_x,
double collision_mask_y,
double collision_mask_z);
87 void setStartGoal(
const std::vector<double> start_state,
const std::vector<double> goal_state);
100 bool isValid(std::vector<double> position)
const;
111 bool updatePath(octomap::OcTree* octree,
double solving_time,
const std::vector<double> start_state,
const std::vector<double> goal_state);
114 og::PathGeometric
getPath()
const;
127 int _planningDimension;
129 ob::SpaceInformationPtr _si;
130 ob::StateSpacePtr _space;
131 og::SimpleSetup _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.
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.
Class for planner for 2D and 3D planning.
bool isValid(std::vector< double > position) const
Check if position is collision-free.