hector_costmap.h
Go to the documentation of this file.
00001 #include <hector_elevation_msgs/ElevationMapMetaData.h>
00002 #include <hector_elevation_msgs/ElevationGrid.h>
00003 
00004 #include <std_msgs/String.h>
00005 #include <nav_msgs/OccupancyGrid.h>
00006 #include <nav_msgs/MapMetaData.h>
00007 
00008 #include <ros/ros.h>
00009 
00010 #include <opencv/cv.h>
00011 #include <opencv/highgui.h>
00012 
00013 #include <cv_bridge/cv_bridge.h>
00014 
00015 #include <pcl/point_types.h>
00016 #include <pcl/filters/crop_box.h>
00017 #include <pcl_ros/point_cloud.h>
00018 #include <pcl_ros/transforms.h>
00019 
00020 #include <hector_map_tools/HectorMapTools.h>
00021 
00022 #include <tf/transform_listener.h>
00023 
00024 #include <dynamic_reconfigure/server.h>
00025 #include <hector_costmap/CostMapCalculationConfig.h>
00026 
00027 class CostMapCalculation{
00028 
00029 public:
00031     CostMapCalculation();
00032 
00034     ~CostMapCalculation();
00035 
00037 
00040     void updateMapParamsCallback(const nav_msgs::MapMetaData& map_meta_data);
00041 
00043 
00046     void callbackElevationMap(const hector_elevation_msgs::ElevationGridConstPtr& elevation_map_msg);
00047 
00049 
00052     void callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
00053 
00055 
00058     void callbackGridMap(const nav_msgs::OccupancyGridConstPtr& grid_map_msg);
00059 
00061 
00064     void sysMessageCallback(const std_msgs::String& string);
00065 
00067 
00071     void dynRecParamCallback(hector_costmap::CostMapCalculationConfig &config, uint32_t level);
00072 
00074 
00077     void timerCallback(const ros::TimerEvent& event);
00078 
00079 private:
00080     ros::NodeHandle nHandle;
00081     ros::NodeHandle pnHandle;
00082 
00083     ros::Publisher pub_cost_map;
00084     ros::Publisher pub_cloud_slice;
00085 
00086     ros::Subscriber sub_elevation_map;
00087     ros::Subscriber sub_grid_map;
00088     ros::Subscriber sub_point_cloud;
00089     ros::Subscriber sub_sysMessage;
00090     ros::Subscriber sub_map_info;
00091 
00092     dynamic_reconfigure::Server<hector_costmap::CostMapCalculationConfig> dyn_rec_server_;
00093 
00094     tf::TransformListener listener;
00095 
00096     ros::Timer timer;
00097 
00098     HectorMapTools::CoordinateTransformer<float> world_map_transform;
00099 
00100     double update_radius_world, initial_free_cells_radius;
00101     double grid_res_z;
00102     int elevation_zero_level;
00103     double max_delta_elevation;
00104     bool use_elevation_map, use_grid_map, use_cloud_map, received_grid_map, received_elevation_map, received_point_cloud, allow_elevation_map_to_clear_occupied_cells;
00105     int max_clear_size;
00106     double costmap_pub_freq;
00107     double slize_min_height, slize_max_height;
00108 
00109     pcl::PointCloud<pcl::PointXYZ>::Ptr sliced_cloud;
00110     nav_msgs::OccupancyGridConstPtr grid_map_msg_;
00111 
00112     nav_msgs::OccupancyGrid cost_map, elevation_cost_map, cloud_cost_map;
00113 
00114     cv::Mat grid_map_, elevation_map_, elevation_map_filtered,elevation_cost_map_;
00115 
00116     std::string cost_map_topic, elevation_map_topic, grid_map_topic, point_cloud_topic, sys_msg_topic;
00117     std::string map_frame_id,local_transform_frame_id;
00118 
00119     Eigen::Vector2i min_index, max_index;
00120 
00122 
00126     bool calculateCostMap_old(char map_level);
00127 
00129 
00133     bool calculateCostMap(char map_level);
00134 
00136 
00141     bool computeWindowIndices(ros::Time time,double update_radius);
00142 };
00143 


hector_costmap
Author(s):
autogenerated on Wed May 8 2019 02:32:03