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