00001 #include <ros/ros.h> 00002 00003 #include <tf/tf.h> 00004 #include <tf/transform_listener.h> 00005 00006 #include <std_msgs/String.h> 00007 00008 #include <hector_elevation_msgs/ElevationMapMetaData.h> 00009 #include <hector_elevation_msgs/ElevationGrid.h> 00010 00011 #include <dynamic_reconfigure/server.h> 00012 #include <hector_elevation_visualization/ElevationVisualizationConfig.h> 00013 00014 #include <visualization_msgs/MarkerArray.h> 00015 00016 #include <eigen3/Eigen/Core> 00017 00018 #include <hector_map_tools/HectorMapTools.h> 00019 00020 00021 class ElevationVisualization{ 00022 00023 public: 00025 ElevationVisualization(); 00026 00028 ~ElevationVisualization(); 00029 00031 00035 void dynRecParamCallback(hector_elevation_visualization::ElevationVisualizationConfig &config, uint32_t level); 00036 00038 00041 void sys_message_callback(const std_msgs::String& string); 00042 00044 00047 void map_callback(const hector_elevation_msgs::ElevationGrid& elevation_map); 00048 00049 private: 00050 ros::NodeHandle nHandle; 00051 00052 ros::Subscriber sub_elevation_map; 00053 ros::Subscriber sub_sys_message_callback; 00054 ros::Publisher map_marker_array_publisher; 00055 00056 dynamic_reconfigure::Server<hector_elevation_visualization::ElevationVisualizationConfig> dyn_rec_server_; 00057 00058 visualization_msgs::MarkerArray map_marker_array_msg; 00059 00060 HectorMapTools::CoordinateTransformer<float> world_map_transform; 00061 00062 std::string elevation_map_frame_id,sys_msg_topic; 00063 00064 int max_height_levels; 00065 00066 double min_height,max_height,color_factor; 00067 00068 bool use_color_map; 00069 00070 std_msgs::ColorRGBA marker_color; 00071 00073 00077 void visualize_map(const hector_elevation_msgs::ElevationGrid& elevation_map, tf::StampedTransform local_map_transform); 00078 00080 00083 static std_msgs::ColorRGBA heightMapColor(double h); 00084 };