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$
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


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