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> 22 #include <octomap_msgs/conversions.h> 23 #include <octomap_msgs/GetOctomap.h> 26 using octomap_msgs::GetOctomap;
28 #include "../OMPL_Planner/OMPL_Planner.hpp" 49 void odomCallback(
const nav_msgs::OdometryPtr &odom_msg);
56 bool findPathToGoal(control_turtlebot::FindPathToGoal::Request& request, control_turtlebot::FindPathToGoal::Response& response);
70 void PublishPath(control_turtlebot::FindPathToGoal::Response& response);
101 octomap::AbstractOcTree* abs_octree_;
102 octomap::OcTree* octree_ =
nullptr;
105 ros::NodeHandle _node_hand;
106 ros::Publisher visual_traj_pub_, path_pub_;
107 ros::Subscriber odom_sub_;
108 ros::ServiceServer service_;
110 GetOctomap::Request req;
111 GetOctomap::Response resp;
112 std::string serv_name =
"/octomap_full";
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_;
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.