00001 #include "pcl_ros/point_cloud.h" 00002 #include "pcl/point_types.h" 00003 00004 #include <pcl_ros/transforms.h> 00005 00006 #include <tf/transform_listener.h> 00007 00008 #include <std_msgs/String.h> 00009 #include <sensor_msgs/point_cloud_conversion.h> 00010 00011 #include <hector_elevation_msgs/ElevationMapMetaData.h> 00012 #include <hector_elevation_msgs/ElevationGrid.h> 00013 00014 #include <nav_msgs/OccupancyGrid.h> 00015 #include <nav_msgs/MapMetaData.h> 00016 00017 #include <hector_map_tools/HectorMapTools.h> 00018 00019 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00020 00021 #include <nodelet/nodelet.h> 00022 00023 namespace hector_elevation_mapping 00024 { 00025 00026 class ElevationMapping : public nodelet::Nodelet{ 00027 00028 public: 00030 virtual void onInit(); 00031 00033 ~ElevationMapping(); 00034 00036 00039 void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointcloud2_sensor_msg); 00040 00042 00045 void sysMessageCallback(const std_msgs::String& string); 00046 00047 00049 00052 void updateParamsCallback(const nav_msgs::MapMetaData& map_meta_data); 00053 00054 00056 00059 void timerCallback(const ros::TimerEvent& event); 00060 00061 private: 00062 HectorMapTools::CoordinateTransformer<float> world_map_transform; 00063 00064 tf::TransformListener listener; 00065 ros::NodeHandle nHandle; 00066 ros::NodeHandle nPrivateHandle; 00067 00068 ros::Subscriber sub_pointCloud; 00069 ros::Subscriber sub_sysMessage; 00070 ros::Subscriber sub_grid_map_info; 00071 00072 ros::Publisher pub_local_map; 00073 ros::Publisher pub_local_map_meta; 00074 ros::Publisher pub_global_map; 00075 ros::Publisher pub_global_map_meta; 00076 00077 ros::Publisher pub_height_update; 00078 00079 ros::Timer timer; 00080 00081 hector_elevation_msgs::ElevationMapMetaData elevation_map_meta; 00082 hector_elevation_msgs::ElevationGrid local_elevation_map; 00083 hector_elevation_msgs::ElevationGrid global_elevation_map; 00084 std::vector<double> cell_variance; 00085 00086 nav_msgs::MapMetaData map_meta; 00087 00088 double sensor_variance; 00089 double max_observable_distance; 00090 00091 bool publish_poseupdate; 00092 int max_height_levels, max_height; 00093 00094 double poseupdate_pub_period; 00095 double poseupdate_height_covariance; 00096 int poseupdate_used_pattern_size; 00097 00098 std::string sensor_frame_id, map_frame_id, local_map_frame_id; 00099 std::string grid_map_topic, local_elevation_map_topic, global_elevation_map_topic, point_cloud_topic, sys_msg_topic, pose_update_topic; 00100 00101 }; 00102 }