$search
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: rawdata.cc 2263 2012-04-08 23:10:01Z jack.oquin $ 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