Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012 The MITRE Corporation 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #include "pcl/pcl_config.h" 00039 00040 #ifndef PCL_IO_HDL_GRABBER_H_ 00041 #define PCL_IO_HDL_GRABBER_H_ 00042 00043 #include <pcl/io/grabber.h> 00044 #include <pcl/io/impl/synchronized_queue.hpp> 00045 #include <pcl/point_types.h> 00046 #include <pcl/point_cloud.h> 00047 #include <boost/asio.hpp> 00048 #include <string> 00049 00050 #define HDL_Grabber_toRadians(x) ((x) * M_PI / 180.0) 00051 00052 namespace pcl 00053 { 00054 00055 /** \brief Grabber for the Velodyne High-Definition-Laser (HDL) 00056 * \author Keven Ring <keven@mitre.org> 00057 * \ingroup io 00058 */ 00059 class PCL_EXPORTS HDLGrabber : public Grabber 00060 { 00061 public: 00062 /** \brief Signal used for a single sector 00063 * Represents 1 corrected packet from the HDL Velodyne 00064 */ 00065 typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyz) ( 00066 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&, 00067 float, float); 00068 /** \brief Signal used for a single sector 00069 * Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB 00070 */ 00071 typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) ( 00072 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&, 00073 float, float); 00074 /** \brief Signal used for a single sector 00075 * Represents 1 corrected packet from the HDL Velodyne with the returned intensity. 00076 */ 00077 typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) ( 00078 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&, 00079 float startAngle, float); 00080 /** \brief Signal used for a 360 degree sweep 00081 * Represents multiple corrected packets from the HDL Velodyne 00082 * This signal is sent when the Velodyne passes angle "0" 00083 */ 00084 typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyz) ( 00085 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&); 00086 /** \brief Signal used for a 360 degree sweep 00087 * Represents multiple corrected packets from the HDL Velodyne with the returned intensity 00088 * This signal is sent when the Velodyne passes angle "0" 00089 */ 00090 typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) ( 00091 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&); 00092 /** \brief Signal used for a 360 degree sweep 00093 * Represents multiple corrected packets from the HDL Velodyne 00094 * This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB 00095 */ 00096 typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) ( 00097 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&); 00098 00099 /** \brief Constructor taking an optional path to an HDL corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368] 00100 * \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL. This parameter is mandatory for the HDL-64, optional for the HDL-32 00101 * \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional 00102 */ 00103 HDLGrabber (const std::string& correctionsFile = "", 00104 const std::string& pcapFile = ""); 00105 00106 /** \brief Constructor taking a pecified IP/port and an optional path to an HDL corrections file. 00107 * \param[in] ipAddress IP Address that should be used to listen for HDL packets 00108 * \param[in] port UDP Port that should be used to listen for HDL packets 00109 * \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL. This field is mandatory for the HDL-64, optional for the HDL-32 00110 */ 00111 HDLGrabber (const boost::asio::ip::address& ipAddress, 00112 const unsigned short port, const std::string& correctionsFile = ""); 00113 00114 /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ 00115 virtual ~HDLGrabber () throw (); 00116 00117 /** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */ 00118 virtual void start (); 00119 00120 /** \brief Stops processing the Velodyne packets, either from the network or PCAP file */ 00121 virtual void stop (); 00122 00123 /** \brief Obtains the name of this I/O Grabber 00124 * \return The name of the grabber 00125 */ 00126 virtual std::string getName () const; 00127 00128 /** \brief Check if the grabber is still running. 00129 * \return TRUE if the grabber is running, FALSE otherwise 00130 */ 00131 virtual bool isRunning () const; 00132 00133 /** \brief Returns the number of frames per second. 00134 */ 00135 virtual float getFramesPerSecond () const; 00136 00137 /** \brief Allows one to filter packets based on the SOURCE IP address and PORT 00138 * This can be used, for instance, if multiple HDL LIDARs are on the same network 00139 */ 00140 void filterPackets (const boost::asio::ip::address& ipAddress, 00141 const unsigned short port = 443); 00142 00143 /** \brief Allows one to customize the colors used for each of the lasers. 00144 */ 00145 void setLaserColorRGB (const pcl::RGB& color, unsigned int laserNumber); 00146 00147 /** \brief Any returns from the HDL with a distance less than this are discarded. 00148 * This value is in meters 00149 * Default: 0.0 00150 */ 00151 void setMinimumDistanceThreshold(float & minThreshold); 00152 00153 /** \brief Any returns from the HDL with a distance greater than this are discarded. 00154 * This value is in meters 00155 * Default: 10000.0 00156 */ 00157 void setMaximumDistanceThreshold(float & maxThreshold); 00158 00159 /** \brief Returns the current minimum distance threshold, in meters 00160 */ 00161 00162 float getMinimumDistanceThreshold(); 00163 00164 /** \brief Returns the current maximum distance threshold, in meters 00165 */ 00166 float getMaximumDistanceThreshold(); 00167 00168 protected: 00169 static const int HDL_DATA_PORT = 2368; 00170 static const int HDL_NUM_ROT_ANGLES = 36001; 00171 static const int HDL_LASER_PER_FIRING = 32; 00172 static const int HDL_MAX_NUM_LASERS = 64; 00173 static const int HDL_FIRING_PER_PKT = 12; 00174 static const boost::asio::ip::address HDL_DEFAULT_NETWORK_ADDRESS; 00175 00176 enum HDLBlock 00177 { 00178 BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff 00179 }; 00180 00181 #pragma pack(push, 1) 00182 typedef struct HDLLaserReturn 00183 { 00184 unsigned short distance; 00185 unsigned char intensity; 00186 } HDLLaserReturn; 00187 #pragma pack(pop) 00188 00189 struct HDLFiringData 00190 { 00191 unsigned short blockIdentifier; 00192 unsigned short rotationalPosition; 00193 HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING]; 00194 }; 00195 00196 struct HDLDataPacket 00197 { 00198 HDLFiringData firingData[HDL_FIRING_PER_PKT]; 00199 unsigned int gpsTimestamp; 00200 unsigned char blank1; 00201 unsigned char blank2; 00202 }; 00203 00204 struct HDLLaserCorrection 00205 { 00206 double azimuthCorrection; 00207 double verticalCorrection; 00208 double distanceCorrection; 00209 double verticalOffsetCorrection; 00210 double horizontalOffsetCorrection; 00211 double sinVertCorrection; 00212 double cosVertCorrection; 00213 double sinVertOffsetCorrection; 00214 double cosVertOffsetCorrection; 00215 }; 00216 00217 private: 00218 static double *cos_lookup_table_; 00219 static double *sin_lookup_table_; 00220 pcl::SynchronizedQueue<unsigned char *> hdl_data_; 00221 boost::asio::ip::udp::endpoint udp_listener_endpoint_; 00222 boost::asio::ip::address source_address_filter_; 00223 unsigned short source_port_filter_; 00224 boost::asio::io_service hdl_read_socket_service_; 00225 boost::asio::ip::udp::socket *hdl_read_socket_; 00226 std::string pcap_file_name_; 00227 boost::thread *queue_consumer_thread_; 00228 boost::thread *hdl_read_packet_thread_; 00229 HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS]; 00230 bool terminate_read_packet_thread_; 00231 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > current_scan_xyz_, 00232 current_sweep_xyz_; 00233 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > current_scan_xyzi_, 00234 current_sweep_xyzi_; 00235 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > current_scan_xyzrgb_, 00236 current_sweep_xyzrgb_; 00237 unsigned int last_azimuth_; 00238 boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_; 00239 boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb>* sweep_xyzrgb_signal_; 00240 boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_; 00241 boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_; 00242 boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb>* scan_xyzrgb_signal_; 00243 boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_; 00244 pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS]; 00245 float min_distance_threshold_; 00246 float max_distance_threshold_; 00247 00248 void processVelodynePackets (); 00249 void enqueueHDLPacket (const unsigned char *data, 00250 std::size_t bytesReceived); 00251 void initialize (const std::string& correctionsFile); 00252 void loadCorrectionsFile (const std::string& correctionsFile); 00253 void loadHDL32Corrections (); 00254 void readPacketsFromSocket (); 00255 #ifdef HAVE_PCAP 00256 void readPacketsFromPcap(); 00257 #endif //#ifdef HAVE_PCAP 00258 void toPointClouds (HDLDataPacket *dataPacket); 00259 void fireCurrentSweep (); 00260 void fireCurrentScan (const unsigned short startAngle, 00261 const unsigned short endAngle); 00262 void computeXYZI (pcl::PointXYZI& pointXYZI, int azimuth, 00263 HDLLaserReturn laserReturn, HDLLaserCorrection correction); 00264 bool isAddressUnspecified (const boost::asio::ip::address& ip_address); 00265 }; 00266 } 00267 00268 #endif /* PCL_IO_HDL_GRABBER_H_ */