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