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  */
67 
68  /** \brief Signal used for a single sector
69  * Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB
70  */
72 
73  using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb [[deprecated("use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead")]]
75 
76  /** \brief Signal used for a single sector
77  * Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
78  */
80 
81  /** \brief Signal used for a 360 degree sweep
82  * Represents multiple corrected packets from the HDL Velodyne
83  * This signal is sent when the Velodyne passes angle "0"
84  */
86 
87  /** \brief Signal used for a 360 degree sweep
88  * Represents multiple corrected packets from the HDL Velodyne with the returned intensity
89  * This signal is sent when the Velodyne passes angle "0"
90  */
92 
93  /** \brief Signal used for a 360 degree sweep
94  * Represents multiple corrected packets from the HDL Velodyne
95  * This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB
96  */
98 
99  using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb [[deprecated("use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead")]]
101 
102  /** \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]
103  * \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
104  * \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional
105  */
106  HDLGrabber (const std::string& correctionsFile = "",
107  const std::string& pcapFile = "");
108 
109  /** \brief Constructor taking a specified IP/port and an optional path to an HDL corrections file.
110  * \param[in] ipAddress IP Address that should be used to listen for HDL packets
111  * \param[in] port UDP Port that should be used to listen for HDL packets
112  * \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
113  */
114  HDLGrabber (const boost::asio::ip::address& ipAddress,
115  const uint16_t port,
116  const std::string& correctionsFile = "");
117 
118  /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
119 
120  ~HDLGrabber () throw ();
121 
122  /** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */
123  void
124  start () override;
125 
126  /** \brief Stops processing the Velodyne packets, either from the network or PCAP file */
127  void
128  stop () override;
129 
130  /** \brief Obtains the name of this I/O Grabber
131  * \return The name of the grabber
132  */
133  std::string
134  getName () const override;
135 
136  /** \brief Check if the grabber is still running.
137  * \return TRUE if the grabber is running, FALSE otherwise
138  */
139  bool
140  isRunning () const override;
141 
142  /** \brief Returns the number of frames per second.
143  */
144  float
145  getFramesPerSecond () const override;
146 
147  /** \brief Allows one to filter packets based on the SOURCE IP address and PORT
148  * This can be used, for instance, if multiple HDL LIDARs are on the same network
149  */
150  void
151  filterPackets (const boost::asio::ip::address& ipAddress,
152  const uint16_t port = 443);
153 
154  /** \brief Allows one to customize the colors used by each laser.
155  * \param[in] color RGB color to set
156  * \param[in] laserNumber Number of laser to set color
157  */
158  void
159  setLaserColorRGB (const pcl::RGB& color,
160  const uint8_t laserNumber);
161 
162  /** \brief Allows one to customize the colors used for each of the lasers.
163  * \param[in] begin begin iterator of RGB color array
164  * \param[in] end end iterator of RGB color array
165  */
166  template<typename IterT> void
167  setLaserColorRGB (const IterT& begin, const IterT& end)
168  {
169  std::copy (begin, end, laser_rgb_mapping_);
170  }
171 
172  /** \brief Any returns from the HDL with a distance less than this are discarded.
173  * This value is in meters
174  * Default: 0.0
175  */
176  void
177  setMinimumDistanceThreshold (float & minThreshold);
178 
179  /** \brief Any returns from the HDL with a distance greater than this are discarded.
180  * This value is in meters
181  * Default: 10000.0
182  */
183  void
184  setMaximumDistanceThreshold (float & maxThreshold);
185 
186  /** \brief Returns the current minimum distance threshold, in meters
187  */
188 
189  float
190  getMinimumDistanceThreshold ();
191 
192  /** \brief Returns the current maximum distance threshold, in meters
193  */
194  float
195  getMaximumDistanceThreshold ();
196 
197  /** \brief Returns the maximum number of lasers
198  */
199  virtual uint8_t
200  getMaximumNumberOfLasers () const;
201 
202  protected:
203  static const uint16_t HDL_DATA_PORT = 2368;
204  static const uint16_t HDL_NUM_ROT_ANGLES = 36001;
205  static const uint8_t HDL_LASER_PER_FIRING = 32;
206  static const uint8_t HDL_MAX_NUM_LASERS = 64;
207  static const uint8_t HDL_FIRING_PER_PKT = 12;
208 
209  enum HDLBlock
210  {
211  BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
212  };
213 
214 #pragma pack(push, 1)
216  {
217  uint16_t distance;
218  uint8_t intensity;
219  };
220 #pragma pack(pop)
221 
223  {
224  uint16_t blockIdentifier;
226  HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING];
227  };
228 
230  {
231  HDLFiringData firingData[HDL_FIRING_PER_PKT];
232  uint32_t gpsTimestamp;
233  uint8_t mode;
234  uint8_t sensorType;
235  };
236 
238  {
248  };
249 
250  HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
251  uint16_t last_azimuth_;
255  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
256  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba>* sweep_xyzrgba_signal_;
257  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
258  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
259  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba>* scan_xyzrgba_signal_;
260  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
261 
262  void
263  fireCurrentSweep ();
264 
265  void
266  fireCurrentScan (const uint16_t startAngle,
267  const uint16_t endAngle);
268  void
269  computeXYZI (pcl::PointXYZI& pointXYZI,
270  uint16_t azimuth,
271  HDLLaserReturn laserReturn,
272  HDLLaserCorrection correction);
273 
274 
275  private:
276  static double *cos_lookup_table_;
277  static double *sin_lookup_table_;
279  boost::asio::ip::udp::endpoint udp_listener_endpoint_;
280  boost::asio::ip::address source_address_filter_;
281  uint16_t source_port_filter_;
282  boost::asio::io_service hdl_read_socket_service_;
283  boost::asio::ip::udp::socket *hdl_read_socket_;
284  std::string pcap_file_name_;
285  std::thread *queue_consumer_thread_;
286  std::thread *hdl_read_packet_thread_;
287  bool terminate_read_packet_thread_;
288  pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
289  float min_distance_threshold_;
290  float max_distance_threshold_;
291 
292  virtual void
293  toPointClouds (HDLDataPacket *dataPacket);
294 
295  virtual boost::asio::ip::address
296  getDefaultNetworkAddress ();
297 
298  void
299  initialize (const std::string& correctionsFile = "");
300 
301  void
302  processVelodynePackets ();
303 
304  void
305  enqueueHDLPacket (const uint8_t *data,
306  std::size_t bytesReceived);
307 
308  void
309  loadCorrectionsFile (const std::string& correctionsFile);
310 
311  void
312  loadHDL32Corrections ();
313 
314  void
315  readPacketsFromSocket ();
316 
317 #ifdef HAVE_PCAP
318  void
319  readPacketsFromPcap();
320 
321 #endif //#ifdef HAVE_PCAP
322 
323  bool
324  isAddressUnspecified (const boost::asio::ip::address& ip_address);
325 
326  };
327 }
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &, float, float) sig_cb_velodyne_hdl_scan_point_cloud_xyzi
Signal used for a single sector Represents 1 corrected packet from the HDL Velodyne with the returned...
Definition: hdl_grabber.h:79
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba > * sweep_xyzrgba_signal_
Definition: hdl_grabber.h:256
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &) sig_cb_velodyne_hdl_sweep_point_cloud_xyzi
Signal used for a 360 degree sweep Represents multiple corrected packets from the HDL Velodyne with t...
Definition: hdl_grabber.h:91
pcl::PointCloud< pcl::PointXYZI >::Ptr current_sweep_xyzi_
Definition: hdl_grabber.h:253
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr current_sweep_xyzrgba_
Definition: hdl_grabber.h:254
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &, float, float) sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba
Signal used for a single sector Represents 1 corrected packet from the HDL Velodyne.
Definition: hdl_grabber.h:71
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyz > * scan_xyz_signal_
Definition: hdl_grabber.h:258
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyz > * sweep_xyz_signal_
Definition: hdl_grabber.h:255
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &) sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba
Signal used for a 360 degree sweep Represents multiple corrected packets from the HDL Velodyne This s...
Definition: hdl_grabber.h:97
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
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &, float, float) sig_cb_velodyne_hdl_scan_point_cloud_xyz
Signal used for a single sector Represents 1 corrected packet from the HDL Velodyne.
Definition: hdl_grabber.h:66
A structure representing RGB color information.
uint16_t last_azimuth_
Definition: hdl_grabber.h:251
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzi > * scan_xyzi_signal_
Definition: hdl_grabber.h:260
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &) sig_cb_velodyne_hdl_sweep_point_cloud_xyz
Signal used for a 360 degree sweep Represents multiple corrected packets from the HDL Velodyne This s...
Definition: hdl_grabber.h:85
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba > * scan_xyzrgba_signal_
Definition: hdl_grabber.h:259
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzi > * sweep_xyzi_signal_
Definition: hdl_grabber.h:257
pcl::PointCloud< pcl::PointXYZ >::Ptr current_sweep_xyz_
Definition: hdl_grabber.h:252