00001 /* -*- mode: C++ -*- 00002 * 00003 * Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson 00004 * Copyright (C) 2009, 2010, 2012 Austin Robot Technology, Jack O'Quin 00005 * 00006 * License: Modified BSD Software License Agreement 00007 * 00008 * $Id$ 00009 */ 00010 00020 #ifndef __VELODYNE_RAWDATA_H 00021 #define __VELODYNE_RAWDATA_H 00022 00023 #include <errno.h> 00024 #include <stdint.h> 00025 #include <string> 00026 #include <boost/format.hpp> 00027 #include <math.h> 00028 00029 #include <ros/ros.h> 00030 #include <pcl_ros/point_cloud.h> 00031 #include <velodyne_msgs/VelodyneScan.h> 00032 #include <velodyne_pointcloud/point_types.h> 00033 #include <velodyne_pointcloud/calibration.h> 00034 00035 namespace velodyne_rawdata 00036 { 00037 // Shorthand typedefs for point cloud representations 00038 typedef velodyne_pointcloud::PointXYZIR VPoint; 00039 typedef pcl::PointCloud<VPoint> VPointCloud; 00040 00044 static const int SIZE_BLOCK = 100; 00045 static const int RAW_SCAN_SIZE = 3; 00046 static const int SCANS_PER_BLOCK = 32; 00047 static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); 00048 00049 static const float ROTATION_RESOLUTION = 0.01f; 00050 static const uint16_t ROTATION_MAX_UNITS = 36000; 00054 static const float DISTANCE_MAX = 130.0f; 00055 static const float DISTANCE_RESOLUTION = 0.002f; 00056 static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX 00057 / DISTANCE_RESOLUTION + 1.0); 00058 static const uint16_t UPPER_BANK = 0xeeff; 00059 static const uint16_t LOWER_BANK = 0xddff; 00060 00061 00063 static const int VLP16_FIRINGS_PER_BLOCK = 2; 00064 static const int VLP16_SCANS_PER_FIRING = 16; 00065 static const int VLP16_BLOCK_TDURATION = 110.592; 00066 static const int VLP16_DSR_TOFFSET = 2.304; 00067 static const int VLP16_FIRING_TOFFSET = 55.296; 00068 00069 00077 typedef struct raw_block 00078 { 00079 uint16_t header; 00080 uint16_t rotation; 00081 uint8_t data[BLOCK_DATA_SIZE]; 00082 } raw_block_t; 00083 00089 union two_bytes 00090 { 00091 uint16_t uint; 00092 uint8_t bytes[2]; 00093 }; 00094 00095 static const int PACKET_SIZE = 1206; 00096 static const int BLOCKS_PER_PACKET = 12; 00097 static const int PACKET_STATUS_SIZE = 4; 00098 static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); 00099 00112 typedef struct raw_packet 00113 { 00114 raw_block_t blocks[BLOCKS_PER_PACKET]; 00115 uint16_t revolution; 00116 uint8_t status[PACKET_STATUS_SIZE]; 00117 } raw_packet_t; 00118 00120 class RawData 00121 { 00122 public: 00123 00124 RawData(); 00125 ~RawData() {} 00126 00138 int setup(ros::NodeHandle private_nh); 00139 00140 void unpack(const velodyne_msgs::VelodynePacket &pkt, VPointCloud &pc); 00141 00142 void setParameters(double min_range, double max_range, double view_direction, 00143 double view_width); 00144 00145 private: 00146 00148 typedef struct { 00149 std::string calibrationFile; 00150 double max_range; 00151 double min_range; 00152 int min_angle; 00153 int max_angle; 00154 00155 double tmp_min_angle; 00156 double tmp_max_angle; 00157 } Config; 00158 Config config_; 00159 00163 velodyne_pointcloud::Calibration calibration_; 00164 float sin_rot_table_[ROTATION_MAX_UNITS]; 00165 float cos_rot_table_[ROTATION_MAX_UNITS]; 00166 00168 void unpack_vlp16(const velodyne_msgs::VelodynePacket &pkt, VPointCloud &pc); 00169 00171 bool pointInRange(float range) 00172 { 00173 return (range >= config_.min_range 00174 && range <= config_.max_range); 00175 } 00176 }; 00177 00178 } // namespace velodyne_rawdata 00179 00180 #endif // __VELODYNE_RAWDATA_H