rawdata.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
00003  *  Copyright (C) 2009, 2010, 2012 Austin Robot Technology, Jack O'Quin
00004  *
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id$
00008  */
00009 
00028 #include <fstream>
00029 
00030 #include <ros/ros.h>
00031 #include <ros/package.h>
00032 #include <angles/angles.h>
00033 
00034 #include <velodyne_pointcloud/rawdata.h>
00035 
00036 namespace velodyne_rawdata
00037 {
00039   //
00040   // RawData base class implementation
00041   //
00043 
00044   RawData::RawData() {}
00045 
00047   int RawData::setup(ros::NodeHandle private_nh)
00048   {
00049     private_nh.param("max_range", config_.max_range,
00050                      (double) velodyne_rawdata::DISTANCE_MAX);
00051     private_nh.param("min_range", config_.min_range, 2.0);
00052     ROS_INFO_STREAM("data ranges to publish: ["
00053                     << config_.min_range << ", "
00054                     << config_.max_range << "]");
00055 
00056     // get path to angles.config file for this device
00057     if (!private_nh.getParam("calibration", config_.calibrationFile))
00058       {
00059         ROS_ERROR_STREAM("No calibration angles specified! Using test values!");
00060 
00061         // have to use something: grab unit test version as a default
00062         std::string pkgPath = ros::package::getPath("velodyne_pointcloud");
00063         config_.calibrationFile = pkgPath + "/params/64e_utexas.yaml";
00064       }
00065 
00066     ROS_INFO_STREAM("correction angles: " << config_.calibrationFile);
00067 
00068     calibration_.read(config_.calibrationFile);
00069     if (!calibration_.initialized) {
00070       ROS_ERROR_STREAM("Unable to open calibration file: " << 
00071           config_.calibrationFile);
00072       return -1;
00073     }
00074 
00075     // Set up cached values for sin and cos of all the possible headings
00076     for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
00077       float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
00078       cos_rot_table_[rot_index] = cosf(rotation);
00079       sin_rot_table_[rot_index] = sinf(rotation);
00080     }
00081    return 0;
00082   }
00083 
00089   void RawData::unpack(const velodyne_msgs::VelodynePacket &pkt,
00090                        VPointCloud &pc)
00091   {
00092     ROS_DEBUG_STREAM("Received packet, time: " << pkt.stamp);
00093 
00094     const raw_packet_t *raw = (const raw_packet_t *) &pkt.data[0];
00095 
00096     for (int i = 0; i < BLOCKS_PER_PACKET; i++) {
00097 
00098       // upper bank lasers are numbered [0..31]
00099       // NOTE: this is a change from the old velodyne_common implementation
00100       int bank_origin = 0;
00101       if (raw->blocks[i].header == LOWER_BANK) {
00102         // lower bank lasers are [32..63]
00103         bank_origin = 32;
00104       }
00105 
00106       for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) {
00107         
00108         float x, y, z;
00109         float intensity;
00110         uint8_t laser_number;       
00111 
00112         laser_number = j + bank_origin;
00113         velodyne_pointcloud::LaserCorrection &corrections = 
00114           calibration_.laser_corrections[laser_number];
00115 
00118         union two_bytes tmp;
00119         tmp.bytes[0] = raw->blocks[i].data[k];
00120         tmp.bytes[1] = raw->blocks[i].data[k+1];
00121 
00122         float distance = tmp.uint * DISTANCE_RESOLUTION;
00123         distance += corrections.dist_correction;
00124 
00125         float cos_vert_angle = corrections.cos_vert_correction;
00126         float sin_vert_angle = corrections.sin_vert_correction;
00127         float cos_rot_correction = corrections.cos_rot_correction;
00128         float sin_rot_correction = corrections.sin_rot_correction;
00129 
00130         // cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
00131         // sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
00132         float cos_rot_angle = 
00133           cos_rot_table_[raw->blocks[i].rotation] * cos_rot_correction + 
00134           sin_rot_table_[raw->blocks[i].rotation] * sin_rot_correction;
00135         float sin_rot_angle = 
00136           sin_rot_table_[raw->blocks[i].rotation] * cos_rot_correction - 
00137           cos_rot_table_[raw->blocks[i].rotation] * sin_rot_correction;
00138 
00139         float horiz_offset = corrections.horiz_offset_correction;
00140         float vert_offset = corrections.vert_offset_correction;
00141 
00142         // Compute the distance in the xy plane (w/o accounting for rotation)
00143         float xy_distance = distance * cos_vert_angle;
00144 
00145         // Calculate temporal X, use absolute value.
00146         float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
00147         // Calculate temporal Y, use absolute value
00148         float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
00149         if (xx < 0) xx=-xx;
00150         if (yy < 0) yy=-yy;
00151   
00152         // Get 2points calibration values,Linear interpolation to get distance
00153         // correction for X and Y, that means distance correction use different
00154         // value at different distance
00155         float distance_corr_x = 0;
00156         float distance_corr_y = 0;
00157         if (corrections.two_pt_correction_available) {
00158           distance_corr_x = 
00159             (corrections.dist_correction - corrections.dist_correction_x)
00160               * (xx - 2.4) / (25.04 - 2.4) 
00161             + corrections.dist_correction_x;
00162           distance_corr_y = 
00163             (corrections.dist_correction - corrections.dist_correction_y)
00164               * (yy - 1.93) / (25.04 - 1.93)
00165             + corrections.dist_correction_y;
00166         }
00167 
00168         float distance_x = distance + distance_corr_x;
00169         xy_distance = distance_x * cos_vert_angle;
00170         x = xy_distance * sin_rot_angle + horiz_offset * cos_rot_angle;
00171 
00172         float distance_y = distance + distance_corr_y;
00173         xy_distance = distance_y * cos_vert_angle;
00174         y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
00175 
00176         z = distance * sin_vert_angle + vert_offset;
00177 
00179         float x_coord = y;
00180         float y_coord = -x;
00181         float z_coord = z;
00182 
00185         float min_intensity = corrections.min_intensity;
00186         float max_intensity = corrections.max_intensity;
00187 
00188         intensity = raw->blocks[i].data[k+2];
00189 
00190         float focal_offset = 256 
00191                            * (1 - corrections.focal_distance / 13100) 
00192                            * (1 - corrections.focal_distance / 13100);
00193         float focal_slope = corrections.focal_slope;
00194         intensity += focal_slope * 
00195           (abs(focal_offset - 256 * (1 - tmp.uint/65535)*(1 - tmp.uint/65535)));
00196         intensity = (intensity < min_intensity) ? min_intensity : intensity;
00197         intensity = (intensity > max_intensity) ? max_intensity : intensity;
00198 
00199         if (pointInRange(distance)) {
00200 
00201           // convert polar coordinates to Euclidean XYZ
00202           VPoint point;
00203           point.ring = corrections.laser_ring;
00204           point.x = x_coord;
00205           point.y = y_coord;
00206           point.z = z_coord;
00207           point.intensity = (uint8_t) intensity;
00208 
00209           // append this point to the cloud
00210           pc.points.push_back(point);
00211           ++pc.width;
00212         }
00213       }
00214     }
00215   }  
00216 
00217 } // namespace velodyne_rawdata


velodyne_pointcloud
Author(s): Jack O'Quin, Piyush Khandelwal, Jesse Vera
autogenerated on Fri Jan 3 2014 12:11:12