Point Cloud Library (PCL)  1.7.1
hdl_grabber.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012 The MITRE Corporation
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #include "pcl/pcl_config.h"
39 
40 #ifndef PCL_IO_HDL_GRABBER_H_
41 #define PCL_IO_HDL_GRABBER_H_
42 
43 #include <pcl/io/grabber.h>
44 #include <pcl/io/impl/synchronized_queue.hpp>
45 #include <pcl/point_types.h>
46 #include <pcl/point_cloud.h>
47 #include <boost/asio.hpp>
48 #include <string>
49 
50 #define HDL_Grabber_toRadians(x) ((x) * M_PI / 180.0)
51 
52 namespace pcl
53 {
54 
55  /** \brief Grabber for the Velodyne High-Definition-Laser (HDL)
56  * \author Keven Ring <keven@mitre.org>
57  * \ingroup io
58  */
59  class PCL_EXPORTS HDLGrabber : public Grabber
60  {
61  public:
62  /** \brief Signal used for a single sector
63  * Represents 1 corrected packet from the HDL Velodyne
64  */
65  typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyz) (
66  const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
67  float, float);
68  /** \brief Signal used for a single sector
69  * Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB
70  */
71  typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (
72  const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
73  float, float);
74  /** \brief Signal used for a single sector
75  * Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
76  */
77  typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (
78  const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
79  float startAngle, float);
80  /** \brief Signal used for a 360 degree sweep
81  * Represents multiple corrected packets from the HDL Velodyne
82  * This signal is sent when the Velodyne passes angle "0"
83  */
84  typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (
85  const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
86  /** \brief Signal used for a 360 degree sweep
87  * Represents multiple corrected packets from the HDL Velodyne with the returned intensity
88  * This signal is sent when the Velodyne passes angle "0"
89  */
90  typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (
91  const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
92  /** \brief Signal used for a 360 degree sweep
93  * Represents multiple corrected packets from the HDL Velodyne
94  * This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB
95  */
96  typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (
97  const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
98 
99  /** \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]
100  * \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
101  * \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional
102  */
103  HDLGrabber (const std::string& correctionsFile = "",
104  const std::string& pcapFile = "");
105 
106  /** \brief Constructor taking a pecified IP/port and an optional path to an HDL corrections file.
107  * \param[in] ipAddress IP Address that should be used to listen for HDL packets
108  * \param[in] port UDP Port that should be used to listen for HDL packets
109  * \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
110  */
111  HDLGrabber (const boost::asio::ip::address& ipAddress,
112  const unsigned short port, const std::string& correctionsFile = "");
113 
114  /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
115  virtual ~HDLGrabber () throw ();
116 
117  /** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */
118  virtual void start ();
119 
120  /** \brief Stops processing the Velodyne packets, either from the network or PCAP file */
121  virtual void stop ();
122 
123  /** \brief Obtains the name of this I/O Grabber
124  * \return The name of the grabber
125  */
126  virtual std::string getName () const;
127 
128  /** \brief Check if the grabber is still running.
129  * \return TRUE if the grabber is running, FALSE otherwise
130  */
131  virtual bool isRunning () const;
132 
133  /** \brief Returns the number of frames per second.
134  */
135  virtual float getFramesPerSecond () const;
136 
137  /** \brief Allows one to filter packets based on the SOURCE IP address and PORT
138  * This can be used, for instance, if multiple HDL LIDARs are on the same network
139  */
140  void filterPackets (const boost::asio::ip::address& ipAddress,
141  const unsigned short port = 443);
142 
143  /** \brief Allows one to customize the colors used for each of the lasers.
144  */
145  void setLaserColorRGB (const pcl::RGB& color, unsigned int laserNumber);
146 
147  /** \brief Any returns from the HDL with a distance less than this are discarded.
148  * This value is in meters
149  * Default: 0.0
150  */
151  void setMinimumDistanceThreshold(float & minThreshold);
152 
153  /** \brief Any returns from the HDL with a distance greater than this are discarded.
154  * This value is in meters
155  * Default: 10000.0
156  */
157  void setMaximumDistanceThreshold(float & maxThreshold);
158 
159  /** \brief Returns the current minimum distance threshold, in meters
160  */
161 
162  float getMinimumDistanceThreshold();
163 
164  /** \brief Returns the current maximum distance threshold, in meters
165  */
166  float getMaximumDistanceThreshold();
167 
168  protected:
169  static const int HDL_DATA_PORT = 2368;
170  static const int HDL_NUM_ROT_ANGLES = 36001;
171  static const int HDL_LASER_PER_FIRING = 32;
172  static const int HDL_MAX_NUM_LASERS = 64;
173  static const int HDL_FIRING_PER_PKT = 12;
174  static const boost::asio::ip::address HDL_DEFAULT_NETWORK_ADDRESS;
175 
176  enum HDLBlock
177  {
178  BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
179  };
180 
181 #pragma pack(push, 1)
182  typedef struct HDLLaserReturn
183  {
184  unsigned short distance;
185  unsigned char intensity;
186  } HDLLaserReturn;
187 #pragma pack(pop)
188 
190  {
191  unsigned short blockIdentifier;
192  unsigned short rotationalPosition;
193  HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING];
194  };
195 
197  {
198  HDLFiringData firingData[HDL_FIRING_PER_PKT];
199  unsigned int gpsTimestamp;
200  unsigned char blank1;
201  unsigned char blank2;
202  };
203 
205  {
215  };
216 
217  private:
218  static double *cos_lookup_table_;
219  static double *sin_lookup_table_;
221  boost::asio::ip::udp::endpoint udp_listener_endpoint_;
222  boost::asio::ip::address source_address_filter_;
223  unsigned short source_port_filter_;
224  boost::asio::io_service hdl_read_socket_service_;
225  boost::asio::ip::udp::socket *hdl_read_socket_;
226  std::string pcap_file_name_;
227  boost::thread *queue_consumer_thread_;
228  boost::thread *hdl_read_packet_thread_;
229  HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
230  bool terminate_read_packet_thread_;
231  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > current_scan_xyz_,
232  current_sweep_xyz_;
233  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > current_scan_xyzi_,
234  current_sweep_xyzi_;
235  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > current_scan_xyzrgb_,
236  current_sweep_xyzrgb_;
237  unsigned int last_azimuth_;
238  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
239  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb>* sweep_xyzrgb_signal_;
240  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
241  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
242  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb>* scan_xyzrgb_signal_;
243  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
244  pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
245  float min_distance_threshold_;
246  float max_distance_threshold_;
247 
248  void processVelodynePackets ();
249  void enqueueHDLPacket (const unsigned char *data,
250  std::size_t bytesReceived);
251  void initialize (const std::string& correctionsFile);
252  void loadCorrectionsFile (const std::string& correctionsFile);
253  void loadHDL32Corrections ();
254  void readPacketsFromSocket ();
255 #ifdef HAVE_PCAP
256  void readPacketsFromPcap();
257 #endif //#ifdef HAVE_PCAP
258  void toPointClouds (HDLDataPacket *dataPacket);
259  void fireCurrentSweep ();
260  void fireCurrentScan (const unsigned short startAngle,
261  const unsigned short endAngle);
262  void computeXYZI (pcl::PointXYZI& pointXYZI, int azimuth,
263  HDLLaserReturn laserReturn, HDLLaserCorrection correction);
264  bool isAddressUnspecified (const boost::asio::ip::address& ip_address);
265  };
266 }
267 
268 #endif /* PCL_IO_HDL_GRABBER_H_ */
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:58
Grabber for the Velodyne High-Definition-Laser (HDL)
Definition: hdl_grabber.h:59
unsigned short rotationalPosition
Definition: hdl_grabber.h:192
A structure representing RGB color information.