rawdata.h
Go to the documentation of this file.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: rawdata.h 2134 2012-02-28 14:32:44Z jack.oquin $ 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 00028 #include <ros/ros.h> 00029 #include <pcl_ros/point_cloud.h> 00030 #include <velodyne_msgs/VelodyneScan.h> 00031 #include <velodyne_pointcloud/point_types.h> 00032 #include <velodyne_pointcloud/calibration.h> 00033 00034 namespace velodyne_rawdata 00035 { 00036 // Shorthand typedefs for point cloud representations 00037 typedef velodyne_pointcloud::PointXYZIR VPoint; 00038 typedef pcl::PointCloud<VPoint> VPointCloud; 00039 00043 static const int SIZE_BLOCK = 100; 00044 static const int RAW_SCAN_SIZE = 3; 00045 static const int SCANS_PER_BLOCK = 32; 00046 static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); 00047 00048 static const float ROTATION_RESOLUTION = 0.01f; 00049 static const uint16_t ROTATION_MAX_UNITS = 36000; 00053 static const float DISTANCE_MAX = 130.0f; 00054 static const float DISTANCE_RESOLUTION = 0.002f; 00055 static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX 00056 / DISTANCE_RESOLUTION + 1.0); 00057 static const uint16_t UPPER_BANK = 0xeeff; 00058 static const uint16_t LOWER_BANK = 0xddff; 00059 00067 typedef struct raw_block 00068 { 00069 uint16_t header; 00070 uint16_t rotation; 00071 uint8_t data[BLOCK_DATA_SIZE]; 00072 } raw_block_t; 00073 00079 union two_bytes 00080 { 00081 uint16_t uint; 00082 uint8_t bytes[2]; 00083 }; 00084 00085 static const int PACKET_SIZE = 1206; 00086 static const int BLOCKS_PER_PACKET = 12; 00087 static const int PACKET_STATUS_SIZE = 4; 00088 static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); 00089 static const int PACKETS_PER_REV = 260; 00090 static const int SCANS_PER_REV = (SCANS_PER_PACKET * PACKETS_PER_REV); 00091 00104 typedef struct raw_packet 00105 { 00106 raw_block_t blocks[BLOCKS_PER_PACKET]; 00107 uint16_t revolution; 00108 uint8_t status[PACKET_STATUS_SIZE]; 00109 } raw_packet_t; 00110 00112 class RawData 00113 { 00114 public: 00115 00116 RawData(); 00117 ~RawData() {} 00118 00130 int setup(ros::NodeHandle private_nh); 00131 00132 void unpack(const velodyne_msgs::VelodynePacket &pkt, VPointCloud &pc); 00133 00134 private: 00135 00137 typedef struct { 00138 std::string calibrationFile; 00139 double max_range; 00140 double min_range; 00141 } Config; 00142 Config config_; 00143 00147 velodyne_pointcloud::Calibration calibration_; 00148 float sin_rot_table_[ROTATION_MAX_UNITS]; 00149 float cos_rot_table_[ROTATION_MAX_UNITS]; 00150 00152 bool pointInRange(float range) 00153 { 00154 return (range >= config_.min_range 00155 && range <= config_.max_range); 00156 } 00157 }; 00158 00159 } // namespace velodyne_rawdata 00160 00161 #endif // __VELODYNE_RAWDATA_H