00001 /* -*- mode: C++ -*- */ 00002 /* Copyright (C) 2010 UT-Austin & Austin Robot Technology, 00003 * David Claridge, Michael Quinlan 00004 * 00005 * License: Modified BSD Software License 00006 */ 00007 00008 00009 #ifndef _HEIGHT_MAP_H_ 00010 #define _HEIGHT_MAP_H_ 00011 00012 #include <ros/ros.h> 00013 #include <pcl_ros/point_cloud.h> 00014 #include <pcl/point_types.h> 00015 00016 namespace velodyne_height_map { 00017 00018 // shorter names for point cloud types in this namespace 00019 typedef pcl::PointXYZI VPoint; 00020 typedef pcl::PointCloud<VPoint> VPointCloud; 00021 00022 class HeightMap 00023 { 00024 public: 00025 00031 HeightMap(ros::NodeHandle node, ros::NodeHandle private_nh); 00032 ~HeightMap(); 00033 00040 void processData(const VPointCloud::ConstPtr &scan); 00041 00042 private: 00043 void constructFullClouds(const VPointCloud::ConstPtr &scan, unsigned npoints, 00044 size_t &obs_count, size_t &empty_count); 00045 void constructGridClouds(const VPointCloud::ConstPtr &scan, unsigned npoints, 00046 size_t &obs_count, size_t &empty_count); 00047 00048 00049 // Parameters that define the grids and the height threshold 00050 // Can be set via the parameter server 00051 int grid_dim_; 00052 double m_per_cell_; 00053 double height_diff_threshold_; 00054 bool full_clouds_; 00055 00056 // Point clouds generated in processData 00057 VPointCloud obstacle_cloud_; 00058 VPointCloud clear_cloud_; 00059 00060 // ROS topics 00061 ros::Subscriber velodyne_scan_; 00062 ros::Publisher obstacle_publisher_; 00063 ros::Publisher clear_publisher_; 00064 }; 00065 00066 } // namespace velodyne_height_map 00067 00068 #endif