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 <vector>
00016
00017 #include <hector_map_tools/HectorMapTools.h>
00018
00019 #include <tf/transform_listener.h>
00020
00021 #include <dynamic_reconfigure/server.h>
00022 #include <hector_costmap/CostMapCalculationConfig.h>
00023
00024
00025
00026 class CostMapCalculation{
00027
00028 public:
00030 CostMapCalculation();
00031
00033 ~CostMapCalculation();
00034
00036
00039 void updateMapParamsCallback(const nav_msgs::MapMetaData& map_meta_data);
00040
00042
00045 void callbackElevationMap(const hector_elevation_msgs::ElevationGridConstPtr& elevation_map_msg);
00046
00048
00051 void callbackGridMap(const nav_msgs::OccupancyGridConstPtr& grid_map_msg);
00052
00054
00057 void sysMessageCallback(const std_msgs::String& string);
00058
00060
00064 void dynRecParamCallback(hector_costmap::CostMapCalculationConfig &config, uint32_t level);
00065
00067
00070 void timerCallback(const ros::TimerEvent& event);
00071
00072 private:
00073 ros::NodeHandle nHandle;
00074
00075 ros::Publisher pub_cost_map;
00076
00077 ros::Subscriber sub_elevation_map;
00078 ros::Subscriber sub_grid_map;
00079 ros::Subscriber sub_sysMessage;
00080 ros::Subscriber sub_map_info;
00081
00082 dynamic_reconfigure::Server<hector_costmap::CostMapCalculationConfig> dyn_rec_server_;
00083
00084 tf::TransformListener listener;
00085
00086 ros::Timer timer;
00087
00088 HectorMapTools::CoordinateTransformer<float> world_map_transform;
00089
00090 double update_radius_world, initial_free_cells_radius;
00091 double grid_res_z;
00092 int elevation_zero_level;
00093 double max_delta_elevation;
00094 bool use_elevation_map, use_grid_map, received_grid_map, received_elevation_map, allow_kinect_to_clear_occupied_cells;
00095 int max_clear_size;
00096 double costmap_pub_freq;
00097
00098 nav_msgs::OccupancyGridConstPtr grid_map_msg_;
00099
00100 nav_msgs::OccupancyGrid cost_map, elevation_cost_map;
00101
00102 cv::Mat grid_map_, elevation_map_, elevation_map_filtered,elevation_cost_map_;
00103
00104 std::string cost_map_topic, elevation_map_topic, grid_map_topic, paramSysMsgTopic;
00105 std::string map_frame_id,local_map_frame_id;
00106
00107 Eigen::Vector2i min_index, max_index;
00108
00110
00114 bool calculateCostMap(char map_level);
00115
00117
00122 bool computeWindowIndices(ros::Time time,double update_radius);
00123 };
00124
