Point Cloud Library (PCL)  1.8.0
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 #include "pcl/pcl_config.h"
40 
41 #ifndef PCL_IO_HDL_GRABBER_H_
42 #define PCL_IO_HDL_GRABBER_H_
43 
44 #include <pcl/io/grabber.h>
45 #include <pcl/io/impl/synchronized_queue.hpp>
46 #include <pcl/point_types.h>
47 #include <pcl/point_cloud.h>
48 #include <boost/asio.hpp>
49 #include <string>
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  /** \brief Signal used for a single sector
71  * Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB
72  */
73  typedef void
74  (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
75  float,
76  float);
77  /** \brief Signal used for a single sector
78  * Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
79  */
80  typedef void
81  (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
82  float startAngle,
83  float);
84  /** \brief Signal used for a 360 degree sweep
85  * Represents multiple corrected packets from the HDL Velodyne
86  * This signal is sent when the Velodyne passes angle "0"
87  */
88  typedef void
89  (sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
90  /** \brief Signal used for a 360 degree sweep
91  * Represents multiple corrected packets from the HDL Velodyne with the returned intensity
92  * This signal is sent when the Velodyne passes angle "0"
93  */
94  typedef void
95  (sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
96  /** \brief Signal used for a 360 degree sweep
97  * Represents multiple corrected packets from the HDL Velodyne
98  * This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB
99  */
100  typedef void
101  (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
102 
103  /** \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]
104  * \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
105  * \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional
106  */
107  HDLGrabber (const std::string& correctionsFile = "",
108  const std::string& pcapFile = "");
109 
110  /** \brief Constructor taking a pecified IP/port and an optional path to an HDL corrections file.
111  * \param[in] ipAddress IP Address that should be used to listen for HDL packets
112  * \param[in] port UDP Port that should be used to listen for HDL packets
113  * \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
114  */
115  HDLGrabber (const boost::asio::ip::address& ipAddress,
116  const unsigned short port,
117  const std::string& correctionsFile = "");
118 
119  /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
120  virtual
121  ~HDLGrabber () throw ();
122 
123  /** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */
124  virtual void
125  start ();
126 
127  /** \brief Stops processing the Velodyne packets, either from the network or PCAP file */
128  virtual void
129  stop ();
130 
131  /** \brief Obtains the name of this I/O Grabber
132  * \return The name of the grabber
133  */
134  virtual std::string
135  getName () const;
136 
137  /** \brief Check if the grabber is still running.
138  * \return TRUE if the grabber is running, FALSE otherwise
139  */
140  virtual bool
141  isRunning () const;
142 
143  /** \brief Returns the number of frames per second.
144  */
145  virtual float
146  getFramesPerSecond () const;
147 
148  /** \brief Allows one to filter packets based on the SOURCE IP address and PORT
149  * This can be used, for instance, if multiple HDL LIDARs are on the same network
150  */
151  void
152  filterPackets (const boost::asio::ip::address& ipAddress,
153  const unsigned short port = 443);
154 
155  /** \brief Allows one to customize the colors used for each of the lasers.
156  */
157  void
158  setLaserColorRGB (const pcl::RGB& color,
159  unsigned int laserNumber);
160 
161  /** \brief Any returns from the HDL with a distance less than this are discarded.
162  * This value is in meters
163  * Default: 0.0
164  */
165  void
166  setMinimumDistanceThreshold (float & minThreshold);
167 
168  /** \brief Any returns from the HDL with a distance greater than this are discarded.
169  * This value is in meters
170  * Default: 10000.0
171  */
172  void
173  setMaximumDistanceThreshold (float & maxThreshold);
174 
175  /** \brief Returns the current minimum distance threshold, in meters
176  */
177 
178  float
179  getMinimumDistanceThreshold ();
180 
181  /** \brief Returns the current maximum distance threshold, in meters
182  */
183  float
184  getMaximumDistanceThreshold ();
185 
186  protected:
187  static const int HDL_DATA_PORT = 2368;
188  static const int HDL_NUM_ROT_ANGLES = 36001;
189  static const int HDL_LASER_PER_FIRING = 32;
190  static const int HDL_MAX_NUM_LASERS = 64;
191  static const int HDL_FIRING_PER_PKT = 12;
192 
193  enum HDLBlock
194  {
195  BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
196  };
197 
198 #pragma pack(push, 1)
199  typedef struct HDLLaserReturn
200  {
201  unsigned short distance;
202  unsigned char intensity;
203  } HDLLaserReturn;
204 #pragma pack(pop)
205 
207  {
208  unsigned short blockIdentifier;
209  unsigned short rotationalPosition;
210  HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING];
211  };
212 
214  {
215  HDLFiringData firingData[HDL_FIRING_PER_PKT];
216  unsigned int gpsTimestamp;
217  unsigned char mode;
218  unsigned char sensorType;
219  };
220 
222  {
232  };
233 
234  HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
235  unsigned int last_azimuth_;
236  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > current_scan_xyz_, current_sweep_xyz_;
237  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > current_scan_xyzi_, current_sweep_xyzi_;
238  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > current_scan_xyzrgb_, current_sweep_xyzrgb_;
239  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
240  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb>* sweep_xyzrgb_signal_;
241  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
242  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
243  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb>* scan_xyzrgb_signal_;
244  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
245 
246  void
247  fireCurrentSweep ();
248 
249  void
250  fireCurrentScan (const unsigned short startAngle,
251  const unsigned short endAngle);
252  void
253  computeXYZI (pcl::PointXYZI& pointXYZI,
254  int azimuth,
255  HDLLaserReturn laserReturn,
256  HDLLaserCorrection correction);
257 
258 
259  private:
260  static double *cos_lookup_table_;
261  static double *sin_lookup_table_;
263  boost::asio::ip::udp::endpoint udp_listener_endpoint_;
264  boost::asio::ip::address source_address_filter_;
265  unsigned short source_port_filter_;
266  boost::asio::io_service hdl_read_socket_service_;
267  boost::asio::ip::udp::socket *hdl_read_socket_;
268  std::string pcap_file_name_;
269  boost::thread *queue_consumer_thread_;
270  boost::thread *hdl_read_packet_thread_;
271  bool terminate_read_packet_thread_;
272  pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
273  float min_distance_threshold_;
274  float max_distance_threshold_;
275 
276  virtual void
277  toPointClouds (HDLDataPacket *dataPacket);
278 
279  virtual boost::asio::ip::address
280  getDefaultNetworkAddress ();
281 
282  void
283  initialize (const std::string& correctionsFile = "");
284 
285  void
286  processVelodynePackets ();
287 
288  void
289  enqueueHDLPacket (const unsigned char *data,
290  std::size_t bytesReceived);
291 
292  void
293  loadCorrectionsFile (const std::string& correctionsFile);
294 
295  void
296  loadHDL32Corrections ();
297 
298  void
299  readPacketsFromSocket ();
300 
301 #ifdef HAVE_PCAP
302  void
303  readPacketsFromPcap();
304 
305 #endif //#ifdef HAVE_PCAP
306 
307  bool
308  isAddressUnspecified (const boost::asio::ip::address& ip_address);
309 
310  };
311 }
312 
313 #endif /* PCL_IO_HDL_GRABBER_H_ */
boost::shared_ptr< pcl::PointCloud< pcl::PointXYZRGBA > > current_sweep_xyzrgb_
Definition: hdl_grabber.h:238
boost::shared_ptr< pcl::PointCloud< pcl::PointXYZI > > current_sweep_xyzi_
Definition: hdl_grabber.h:237
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyz > * scan_xyz_signal_
Definition: hdl_grabber.h:242
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyz > * sweep_xyz_signal_
Definition: hdl_grabber.h:239
boost::shared_ptr< pcl::PointCloud< pcl::PointXYZ > > current_sweep_xyz_
Definition: hdl_grabber.h:236
Grabber for the Velodyne High-Definition-Laser (HDL)
Definition: hdl_grabber.h:60
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:58
A structure representing RGB color information.
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb > * scan_xyzrgb_signal_
Definition: hdl_grabber.h:243
Defines all the PCL implemented PointT point type structures.
unsigned int last_azimuth_
Definition: hdl_grabber.h:235
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzi > * scan_xyzi_signal_
Definition: hdl_grabber.h:244
unsigned short rotationalPosition
Definition: hdl_grabber.h:209
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzi > * sweep_xyzi_signal_
Definition: hdl_grabber.h:241
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb > * sweep_xyzrgb_signal_
Definition: hdl_grabber.h:240