00001 #ifndef SBPL_LATTICE_PLANNER_H 00002 #define SBPL_LATTICE_PLANNER_H 00003 00004 #include <iostream> 00005 #include <vector> 00006 00007 using namespace std; 00008 00009 // ROS 00010 #include <ros/ros.h> 00011 #include <geometry_msgs/PoseStamped.h> 00012 00013 // Costmap used for the map representation 00014 #include <costmap_2d/costmap_2d_ros.h> 00015 00016 // sbpl headers 00017 #include <sbpl/headers.h> 00018 00019 // global representation 00020 #include <nav_core/base_global_planner.h> 00021 00022 namespace sbpl_lattice_planner{ 00023 00024 class SBPLLatticePlanner : public nav_core::BaseGlobalPlanner{ 00025 public: 00026 00030 SBPLLatticePlanner(); 00031 00032 00038 SBPLLatticePlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 00039 00040 00046 virtual void initialize(std::string name, 00047 costmap_2d::Costmap2DROS* costmap_ros); 00048 00056 virtual bool makePlan(const geometry_msgs::PoseStamped& start, 00057 const geometry_msgs::PoseStamped& goal, 00058 std::vector<geometry_msgs::PoseStamped>& plan); 00059 00060 virtual ~SBPLLatticePlanner(){}; 00061 00062 private: 00063 unsigned char costMapCostToSBPLCost(unsigned char newcost); 00064 void publishStats(int solution_cost, int solution_size, 00065 const geometry_msgs::PoseStamped& start, 00066 const geometry_msgs::PoseStamped& goal); 00067 00068 unsigned char computeCircumscribedCost(); 00069 00070 bool initialized_; 00071 00072 SBPLPlanner* planner_; 00073 EnvironmentNAVXYTHETALAT* env_; 00074 00075 std::string planner_type_; 00077 double allocated_time_; 00078 double initial_epsilon_; 00080 std::string environment_type_; 00081 std::string cost_map_topic_; 00083 bool forward_search_; 00084 std::string primitive_filename_; 00085 int force_scratch_limit_; 00087 unsigned char lethal_obstacle_; 00088 unsigned char inscribed_inflated_obstacle_; 00089 unsigned char circumscribed_cost_; 00090 unsigned char sbpl_cost_multiplier_; 00091 00092 std::string name_; 00093 costmap_2d::Costmap2DROS* costmap_ros_; 00094 std::vector<geometry_msgs::Point> footprint_; 00095 unsigned int current_env_width_; 00096 unsigned int current_env_height_; 00097 00098 ros::Publisher plan_pub_; 00099 ros::Publisher stats_publisher_; 00100 }; 00101 }; 00102 00103 #endif 00104