OMPL Plannification
Path planning with OMPL
ROS_OMPL_Planner.hpp
1 
9 #include <ros/ros.h>
10 #include <visualization_msgs/Marker.h>
11 #include <nav_msgs/Odometry.h>
12 #include <std_msgs/Bool.h>
13 #include <geometry_msgs/PoseArray.h>
14 #include <ros/package.h>
15 #include <std_srvs/Empty.h>
16 #include <tf/message_filter.h>
17 #include <tf/transform_listener.h>
18 #include <control_turtlebot/FindPathToGoal.h>
19 #include <geometry_msgs/Pose2D.h>
20 #include <geometry_msgs/Pose.h>
21 
22 #include <octomap_msgs/conversions.h>
23 #include <octomap_msgs/GetOctomap.h>
24 
25 //ROS-Octomap interface
26 using octomap_msgs::GetOctomap;
27 
28 #include "../OMPL_Planner/OMPL_Planner.hpp"
29 
35 {
36  public:
38 
43  ROS_OMPL_Planner(int dimension);
44 
46  bool updateMap();
47 
49  void odomCallback(const nav_msgs::OdometryPtr &odom_msg);
50 
52 
56  bool findPathToGoal(control_turtlebot::FindPathToGoal::Request& request, control_turtlebot::FindPathToGoal::Response& response);
57 
59 
62  void visualizeRRT(const og::SimpleSetup simple_setup);
63 
65 
70  void PublishPath(control_turtlebot::FindPathToGoal::Response& response);
71 
73 
78  void PublishPath();
79 
81  bool getObstacleFlag() const;
82 
84 
89  bool updatePath();
90 
92 
96 
97  private:
98  OMPL_Planner planner;
100  //Octomap
101  octomap::AbstractOcTree* abs_octree_;
102  octomap::OcTree* octree_ = nullptr;
104  // *** ROS ***
105  ros::NodeHandle _node_hand;
106  ros::Publisher visual_traj_pub_, path_pub_;
107  ros::Subscriber odom_sub_;
108  ros::ServiceServer service_;
109  // Octomap server
110  GetOctomap::Request req;
111  GetOctomap::Response resp;
112  std::string serv_name = "/octomap_full";
114  bool _obstacle_flag;
115  int planningDimension;
116  double solving_time_,planning_depth_limit;
117  std::string planner_name_;
118  std::vector<double> current_position_, goal_state;
119  std::vector<double> planning_bounds_x_, planning_bounds_y_, planning_bounds_z_;
121  Checking_Box collision_mask;
122 };
void visualizeRRT(const og::SimpleSetup simple_setup)
Procedure to visualize the resulting path.
ROS implementation class for OMPL_Planner.
Collision Mask : virtual representation of the drone as a box with x,y,z length.
bool updatePath()
Update the path.
~ROS_OMPL_Planner()
Destructor.
bool updateMap()
Request and load the actual map from the Octomap server.
void PublishPath()
Publish the resulting path.
ROS_OMPL_Planner(int dimension)
Constructor.
bool getObstacleFlag() const
Return the flag representing the validity of the current path.
void odomCallback(const nav_msgs::OdometryPtr &odom_msg)
Callback for getting current vehicle position.
Class for planner for 2D and 3D planning.
bool findPathToGoal(control_turtlebot::FindPathToGoal::Request &request, control_turtlebot::FindPathToGoal::Response &response)
Find a path.