heightmap.cpp
Go to the documentation of this file.
00001 /*  Copyright (C) 2010 UT-Austin &  Austin Robot Technology,
00002  *  David Claridge, Michael Quinlan
00003  *  Copyright (C) 2012 Jack O'Quin
00004  * 
00005  *  License: Modified BSD Software License 
00006  */
00007 
00033 #include <velodyne_height_map/heightmap.h>
00034 
00035 namespace velodyne_height_map {
00036 
00037 #define MIN(x,y) ((x) < (y) ? (x) : (y))
00038 #define MAX(x,y) ((x) > (y) ? (x) : (y))
00039 
00040 HeightMap::HeightMap(ros::NodeHandle node, ros::NodeHandle priv_nh)
00041 {
00042   // get parameters using private node handle
00043   priv_nh.param("cell_size", m_per_cell_, 0.5);
00044   priv_nh.param("full_clouds", full_clouds_, false);
00045   priv_nh.param("grid_dimensions", grid_dim_, 320);
00046   priv_nh.param("height_threshold", height_diff_threshold_, 0.25);
00047   
00048   ROS_INFO_STREAM("height map parameters: "
00049                   << grid_dim_ << "x" << grid_dim_ << ", "
00050                   << m_per_cell_ << "m cells, "
00051                   << height_diff_threshold_ << "m threshold, "
00052                   << (full_clouds_? "": "not ") << "publishing full clouds");
00053 
00054   // Set up publishers  
00055   obstacle_publisher_ = node.advertise<VPointCloud>("velodyne_obstacles",1);
00056   clear_publisher_ = node.advertise<VPointCloud>("velodyne_clear",1);  
00057 
00058   // subscribe to Velodyne data points
00059   velodyne_scan_ = node.subscribe("velodyne_points", 10,
00060                                   &HeightMap::processData, this,
00061                                   ros::TransportHints().tcpNoDelay(true));
00062 }
00063 
00064 HeightMap::~HeightMap() {}
00065 
00066 void HeightMap::constructFullClouds(const VPointCloud::ConstPtr &scan,
00067                                     unsigned npoints, size_t &obs_count,
00068                                     size_t &empty_count)
00069 {
00070   float min[grid_dim_][grid_dim_];
00071   float max[grid_dim_][grid_dim_];
00072   bool init[grid_dim_][grid_dim_];
00073   memset(&init, 0, grid_dim_*grid_dim_);
00074   
00075   // build height map
00076   for (unsigned i = 0; i < npoints; ++i) {
00077     int x = ((grid_dim_/2)+scan->points[i].x/m_per_cell_);
00078     int y = ((grid_dim_/2)+scan->points[i].y/m_per_cell_);
00079     if (x >= 0 && x < grid_dim_ && y >= 0 && y < grid_dim_) {
00080       if (!init[x][y]) {
00081         min[x][y] = scan->points[i].z;
00082         max[x][y] = scan->points[i].z;
00083         init[x][y] = true;
00084       } else {
00085         min[x][y] = MIN(min[x][y], scan->points[i].z);
00086         max[x][y] = MAX(max[x][y], scan->points[i].z);
00087       }
00088     }
00089   }
00090 
00091   // display points where map has height-difference > threshold
00092   for (unsigned i = 0; i < npoints; ++i) {
00093     int x = ((grid_dim_/2)+scan->points[i].x/m_per_cell_);
00094     int y = ((grid_dim_/2)+scan->points[i].y/m_per_cell_);
00095     if (x >= 0 && x < grid_dim_ && y >= 0 && y < grid_dim_ && init[x][y]) {
00096       if ((max[x][y] - min[x][y] > height_diff_threshold_) ) {   
00097         obstacle_cloud_.points[obs_count].x = scan->points[i].x;
00098         obstacle_cloud_.points[obs_count].y = scan->points[i].y;
00099         obstacle_cloud_.points[obs_count].z = scan->points[i].z;
00100         //obstacle_cloud_.channels[0].values[obs_count] = (float) scan->points[i].intensity;
00101         obs_count++;
00102       } else {
00103         clear_cloud_.points[empty_count].x = scan->points[i].x;
00104         clear_cloud_.points[empty_count].y = scan->points[i].y;
00105         clear_cloud_.points[empty_count].z = scan->points[i].z;
00106         //clear_cloud_.channels[0].values[empty_count] = (float) scan->points[i].intensity;
00107         empty_count++;
00108       }
00109     }
00110   }
00111 }
00112 
00113 void HeightMap::constructGridClouds(const VPointCloud::ConstPtr &scan,
00114                                     unsigned npoints, size_t &obs_count,
00115                                     size_t &empty_count)
00116 {
00117   float min[grid_dim_][grid_dim_];
00118   float max[grid_dim_][grid_dim_];
00119   float num_obs[grid_dim_][grid_dim_];
00120   float num_clear[grid_dim_][grid_dim_];
00121   bool init[grid_dim_][grid_dim_];
00122 
00123   //memset(&init, 0, grid_dim_*grid_dim_);
00124   
00125   for (int x = 0; x < grid_dim_; x++) {
00126     for (int y = 0; y < grid_dim_; y++) {
00127       init[x][y]=false;
00128       num_obs[x][y]=0;
00129       num_clear[x][y]=0;
00130     }
00131   }
00132 
00133   // build height map
00134   for (unsigned i = 0; i < npoints; ++i) {
00135     int x = ((grid_dim_/2)+scan->points[i].x/m_per_cell_);
00136     int y = ((grid_dim_/2)+scan->points[i].y/m_per_cell_);
00137     if (x >= 0 && x < grid_dim_ && y >= 0 && y < grid_dim_) {
00138       if (!init[x][y]) {
00139         min[x][y] = scan->points[i].z;
00140         max[x][y] = scan->points[i].z;
00141         num_obs[x][y] = 0;
00142         num_clear[x][y] = 0;
00143         init[x][y] = true;
00144       } else {
00145         min[x][y] = MIN(min[x][y], scan->points[i].z);
00146         max[x][y] = MAX(max[x][y], scan->points[i].z);
00147       }
00148     }
00149   }
00150 
00151   // calculate number of obstacles in each cell
00152   for (unsigned i = 0; i < npoints; ++i) {
00153     int x = ((grid_dim_/2)+scan->points[i].x/m_per_cell_);
00154     int y = ((grid_dim_/2)+scan->points[i].y/m_per_cell_);
00155     if (x >= 0 && x < grid_dim_ && y >= 0 && y < grid_dim_ && init[x][y]) {
00156       if ((max[x][y] - min[x][y] > height_diff_threshold_) ) {  
00157         num_obs[x][y]++;
00158       } else {
00159         num_clear[x][y]++;
00160       }
00161     }
00162   }
00163 
00164   // create clouds from grid
00165   double grid_offset=grid_dim_/2.0*m_per_cell_;
00166   for (int x = 0; x < grid_dim_; x++) {
00167     for (int y = 0; y < grid_dim_; y++) {
00168       if (num_obs[x][y]>0) {
00169 
00170         obstacle_cloud_.points[obs_count].x = -grid_offset + (x*m_per_cell_+m_per_cell_/2.0);
00171         obstacle_cloud_.points[obs_count].y = -grid_offset + (y*m_per_cell_+m_per_cell_/2.0);
00172         obstacle_cloud_.points[obs_count].z = height_diff_threshold_;
00173         //obstacle_cloud_.channels[0].values[obs_count] = (float) 255.0;
00174         obs_count++;
00175       }
00176       if (num_clear[x][y]>0) {
00177         clear_cloud_.points[empty_count].x = -grid_offset + (x*m_per_cell_+m_per_cell_/2.0);
00178         clear_cloud_.points[empty_count].y = -grid_offset + (y*m_per_cell_+m_per_cell_/2.0);
00179         clear_cloud_.points[empty_count].z = height_diff_threshold_;
00180         //clear_cloud_.channels[0].values[empty_count] = (float) 255.0;
00181         empty_count++;
00182       }
00183     }
00184   }
00185 }
00186 
00188 void HeightMap::processData(const VPointCloud::ConstPtr &scan)
00189 {
00190   if ((obstacle_publisher_.getNumSubscribers() == 0)
00191       && (clear_publisher_.getNumSubscribers() == 0))
00192     return;
00193   
00194   // pass along original time stamp and frame ID
00195   obstacle_cloud_.header.stamp = scan->header.stamp;
00196   obstacle_cloud_.header.frame_id = scan->header.frame_id;
00197 
00198   // pass along original time stamp and frame ID
00199   clear_cloud_.header.stamp = scan->header.stamp;
00200   clear_cloud_.header.frame_id = scan->header.frame_id;
00201 
00202   // set the exact point cloud size -- the vectors should already have
00203   // enough space
00204   size_t npoints = scan->points.size();
00205   obstacle_cloud_.points.resize(npoints);
00206   //obstacle_cloud_.channels[0].values.resize(npoints);
00207 
00208   clear_cloud_.points.resize(npoints);
00209   //clear_cloud_.channels[0].values.resize(npoints);
00210 
00211   size_t obs_count=0;
00212   size_t empty_count=0;
00213   // either return full point cloud or a discretized version
00214   if (full_clouds_)
00215     constructFullClouds(scan,npoints,obs_count, empty_count);
00216   else
00217     constructGridClouds(scan,npoints,obs_count, empty_count);
00218   
00219   obstacle_cloud_.points.resize(obs_count);
00220   //obstacle_cloud_.channels[0].values.resize(obs_count);
00221 
00222   clear_cloud_.points.resize(empty_count);
00223   //clear_cloud_.channels[0].values.resize(empty_count);
00224   
00225   if (obstacle_publisher_.getNumSubscribers() > 0)
00226     obstacle_publisher_.publish(obstacle_cloud_);
00227 
00228   if (clear_publisher_.getNumSubscribers() > 0)
00229     clear_publisher_.publish(clear_cloud_);
00230 }
00231 
00232 } // namespace velodyne_height_map


velodyne_height_map
Author(s): David Claridge, Michael Quinlan
autogenerated on Mon Oct 6 2014 08:36:38