sbpl_dynamic_env_global_planner.h
Go to the documentation of this file.00001 #include <iostream> 00002 #include <vector> 00003 00005 #include <ros/ros.h> 00006 00008 #include <tf/tf.h> 00009 00010 // Costmap used for the map representation 00011 #include <costmap_2d/costmap_2d_ros.h> 00012 00013 // sbpl headers 00014 #include <sbpl_dynamic_planner/sbpl_dynamicObstacles.h> 00015 #include <sbpl_dynamic_planner/DiscreteSpaceTimeInformation.h> 00016 #include <sbpl_dynamic_planner/DiscreteSpaceTimeIntervalInformation.h> 00017 #include <sbpl_dynamic_planner/weightedAStar.h> 00018 #include <sbpl_dynamic_planner/intervalPlanner.h> 00019 00020 //global representation 00021 #include <nav_core/base_global_planner.h> 00022 00023 #include <dynamic_obs_msgs/DynamicObstacles.h> 00024 #include <actionlib_msgs/GoalStatusArray.h> 00025 00026 class SBPLDynEnvGlobalPlanner : public nav_core::BaseGlobalPlanner 00027 { 00028 public: 00029 00033 SBPLDynEnvGlobalPlanner(); 00034 00035 00041 SBPLDynEnvGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 00042 00043 00049 virtual void initialize(std::string name, 00050 costmap_2d::Costmap2DROS* costmap_ros); 00051 00059 virtual bool makePlan(const geometry_msgs::PoseStamped& start, 00060 const geometry_msgs::PoseStamped& goal, 00061 std::vector<geometry_msgs::PoseStamped>& plan); 00062 00063 virtual ~SBPLDynEnvGlobalPlanner(){}; 00064 00065 private: 00066 void dynamicObstacleCallback(const dynamic_obs_msgs::DynamicObstaclesConstPtr& msg); 00067 ros::Time getDynamicObstacles(); 00068 void moveBaseStatusCallback(const actionlib_msgs::GoalStatusArrayConstPtr& msg); 00069 void visualizeExpansions(); 00070 00071 vector<SBPL_DynamicObstacle_t>* sensor_dynObs; 00072 vector<SBPL_DynamicObstacle_t>* current_dynObs; 00073 vector<SBPL_DynamicObstacle_t>* plan_dynObs; 00074 ros::Time current_dynObs_timestamp; 00075 ros::Time plan_dynObs_timestamp; 00076 ros::Subscriber dynObs_sub; 00077 ros::Subscriber moveBaseStatus_sub; 00078 00079 00080 bool initialized_; 00081 00082 IntervalPlanner* planner; 00083 DiscreteSpaceTimeIntervalInformation* env; 00084 00085 tf::TransformListener tf_; 00087 std::string planner_type_; 00089 double allocated_time_; 00090 double initial_epsilon_; 00091 double decrease_epsilon_; 00092 double time_resolution; 00093 double dyn_obs_pad_costmap_removal; 00094 double temporal_padding; 00095 bool remove_dynObs_from_costmap; 00096 00097 std::string environment_type_; 00098 std::string cost_map_topic_; 00100 bool forward_search_; 00101 std::string primitive_filename_; 00102 int force_scratch_limit_; 00105 costmap_2d::Costmap2DROS* costmap_ros_; 00106 costmap_2d::Costmap2D cost_map_; 00108 ros::Publisher plan_pub_; 00109 ros::Publisher marker_pub; 00110 ros::Publisher goal_pub; 00111 geometry_msgs::PoseStamped prevGoal; 00112 bool pathDone; 00113 00114 std::vector<geometry_msgs::Point> footprint_; 00115 00116 boost::recursive_mutex lock_; 00118 pthread_mutex_t m; 00119 };