Point Cloud Library (PCL)  1.11.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 #pragma once
40 
41 #include "pcl/pcl_config.h"
42 #include <pcl/pcl_macros.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 #include <thread>
51 
52 #define HDL_Grabber_toRadians(x) ((x) * M_PI / 180.0)
53 
54 namespace pcl
55 {
56 
57  /** \brief Grabber for the Velodyne High-Definition-Laser (HDL)
58  * \author Keven Ring <keven@mitre.org>
59  * \ingroup io
60  */
62  {
63  public:
64  /** \brief Signal used for a single sector
65  * Represents 1 corrected packet from the HDL Velodyne
66  */
68 
69  /** \brief Signal used for a single sector
70  * Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB
71  */
73 
74  using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb PCL_DEPRECATED(1, 12, "use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead")
76 
77  /** \brief Signal used for a single sector
78  * Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
79  */
81 
82  /** \brief Signal used for a 360 degree sweep
83  * Represents multiple corrected packets from the HDL Velodyne
84  * This signal is sent when the Velodyne passes angle "0"
85  */
87 
88  /** \brief Signal used for a 360 degree sweep
89  * Represents multiple corrected packets from the HDL Velodyne with the returned intensity
90  * This signal is sent when the Velodyne passes angle "0"
91  */
93 
94  /** \brief Signal used for a 360 degree sweep
95  * Represents multiple corrected packets from the HDL Velodyne
96  * This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB
97  */
99 
100  using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb PCL_DEPRECATED(1, 12, "use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead")
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 specified 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 std::uint16_t port,
117  const std::string& correctionsFile = "");
118 
119  /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
120 
121  ~HDLGrabber () noexcept;
122 
123  /** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */
124  void
125  start () override;
126 
127  /** \brief Stops processing the Velodyne packets, either from the network or PCAP file */
128  void
129  stop () override;
130 
131  /** \brief Obtains the name of this I/O Grabber
132  * \return The name of the grabber
133  */
134  std::string
135  getName () const override;
136 
137  /** \brief Check if the grabber is still running.
138  * \return TRUE if the grabber is running, FALSE otherwise
139  */
140  bool
141  isRunning () const override;
142 
143  /** \brief Returns the number of frames per second.
144  */
145  float
146  getFramesPerSecond () const override;
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 std::uint16_t port = 443);
154 
155  /** \brief Allows one to customize the colors used by each laser.
156  * \param[in] color RGB color to set
157  * \param[in] laserNumber Number of laser to set color
158  */
159  void
160  setLaserColorRGB (const pcl::RGB& color,
161  const std::uint8_t laserNumber);
162 
163  /** \brief Allows one to customize the colors used for each of the lasers.
164  * \param[in] begin begin iterator of RGB color array
165  * \param[in] end end iterator of RGB color array
166  */
167  template<typename IterT> void
168  setLaserColorRGB (const IterT& begin, const IterT& end)
169  {
170  std::copy (begin, end, laser_rgb_mapping_);
171  }
172 
173  /** \brief Any returns from the HDL with a distance less than this are discarded.
174  * This value is in meters
175  * Default: 0.0
176  */
177  void
178  setMinimumDistanceThreshold (float & minThreshold);
179 
180  /** \brief Any returns from the HDL with a distance greater than this are discarded.
181  * This value is in meters
182  * Default: 10000.0
183  */
184  void
185  setMaximumDistanceThreshold (float & maxThreshold);
186 
187  /** \brief Returns the current minimum distance threshold, in meters
188  */
189 
190  float
192 
193  /** \brief Returns the current maximum distance threshold, in meters
194  */
195  float
197 
198  /** \brief Returns the maximum number of lasers
199  */
200  virtual std::uint8_t
202 
203  protected:
204  static const std::uint16_t HDL_DATA_PORT = 2368;
205  static const std::uint16_t HDL_NUM_ROT_ANGLES = 36001;
206  static const std::uint8_t HDL_LASER_PER_FIRING = 32;
207  static const std::uint8_t HDL_MAX_NUM_LASERS = 64;
208  static const std::uint8_t HDL_FIRING_PER_PKT = 12;
209 
210  enum HDLBlock
211  {
212  BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
213  };
214 
215 #pragma pack(push, 1)
217  {
220  };
221 #pragma pack(pop)
222 
224  {
227  HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING];
228  };
229 
231  {
232  HDLFiringData firingData[HDL_FIRING_PER_PKT];
236  };
237 
239  {
249  };
250 
251  HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
256  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
257  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba>* sweep_xyzrgba_signal_;
258  boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
259  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
260  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba>* scan_xyzrgba_signal_;
261  boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
262 
263  void
265 
266  void
267  fireCurrentScan (const std::uint16_t startAngle,
268  const std::uint16_t endAngle);
269  void
271  std::uint16_t azimuth,
272  HDLLaserReturn laserReturn,
273  HDLLaserCorrection correction) const;
274 
275 
276  private:
277  static double *cos_lookup_table_;
278  static double *sin_lookup_table_;
280  boost::asio::ip::udp::endpoint udp_listener_endpoint_;
281  boost::asio::ip::address source_address_filter_;
282  std::uint16_t source_port_filter_;
283  boost::asio::io_service hdl_read_socket_service_;
284  boost::asio::ip::udp::socket *hdl_read_socket_;
285  std::string pcap_file_name_;
286  std::thread *queue_consumer_thread_;
287  std::thread *hdl_read_packet_thread_;
288  bool terminate_read_packet_thread_;
289  pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
290  float min_distance_threshold_;
291  float max_distance_threshold_;
292 
293  virtual void
294  toPointClouds (HDLDataPacket *dataPacket);
295 
296  virtual boost::asio::ip::address
297  getDefaultNetworkAddress ();
298 
299  void
300  initialize (const std::string& correctionsFile = "");
301 
302  void
303  processVelodynePackets ();
304 
305  void
306  enqueueHDLPacket (const std::uint8_t *data,
307  std::size_t bytesReceived);
308 
309  void
310  loadCorrectionsFile (const std::string& correctionsFile);
311 
312  void
313  loadHDL32Corrections ();
314 
315  void
316  readPacketsFromSocket ();
317 
318 #ifdef HAVE_PCAP
319  void
320  readPacketsFromPcap();
321 
322 #endif //#ifdef HAVE_PCAP
323 
324  bool
325  isAddressUnspecified (const boost::asio::ip::address& ip_address);
326 
327  };
328 }
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
Definition: convolution.h:46
point_types.h
pcl::uint32_t
std::uint32_t uint32_t
Definition: types.h:58
pcl::HDLGrabber::getMinimumDistanceThreshold
float getMinimumDistanceThreshold()
Returns the current minimum distance threshold, in meters.
pcl::HDLGrabber::HDLFiringData::rotationalPosition
std::uint16_t rotationalPosition
Definition: hdl_grabber.h:226
pcl::HDLGrabber::sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb
sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb
Definition: hdl_grabber.h:101
pcl::HDLGrabber::current_sweep_xyz_
pcl::PointCloud< pcl::PointXYZ >::Ptr current_sweep_xyz_
Definition: hdl_grabber.h:253
pcl::HDLGrabber::getMaximumDistanceThreshold
float getMaximumDistanceThreshold()
Returns the current maximum distance threshold, in meters.
pcl::HDLGrabber::fireCurrentScan
void fireCurrentScan(const std::uint16_t startAngle, const std::uint16_t endAngle)
pcl::HDLGrabber::HDLLaserCorrection::distanceCorrection
double distanceCorrection
Definition: hdl_grabber.h:242
pcl::HDLGrabber::HDLFiringData
Definition: hdl_grabber.h:224
pcl::HDLGrabber::HDLLaserReturn
Definition: hdl_grabber.h:217
pcl::HDLGrabber::HDLDataPacket::gpsTimestamp
std::uint32_t gpsTimestamp
Definition: hdl_grabber.h:233
pcl::HDLGrabber::~HDLGrabber
~HDLGrabber() noexcept
virtual Destructor inherited from the Grabber interface.
pcl::HDLGrabber::current_sweep_xyzi_
pcl::PointCloud< pcl::PointXYZI >::Ptr current_sweep_xyzi_
Definition: hdl_grabber.h:254
pcl::HDLGrabber::HDLFiringData::blockIdentifier
std::uint16_t blockIdentifier
Definition: hdl_grabber.h:225
pcl::PointXYZI
Definition: point_types.hpp:465
pcl::HDLGrabber::HDLLaserCorrection::verticalCorrection
double verticalCorrection
Definition: hdl_grabber.h:241
pcl::HDLGrabber::sweep_xyz_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyz > * sweep_xyz_signal_
Definition: hdl_grabber.h:256
boost
Definition: boost_graph.h:47
pcl::HDLGrabber::HDLLaserCorrection
Definition: hdl_grabber.h:239
pcl::uint16_t
std::uint16_t uint16_t
Definition: types.h:56
pcl::HDLGrabber::HDLGrabber
HDLGrabber(const boost::asio::ip::address &ipAddress, const std::uint16_t port, const std::string &correctionsFile="")
Constructor taking a specified IP/port and an optional path to an HDL corrections file.
pcl::HDLGrabber::scan_xyzi_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzi > * scan_xyzi_signal_
Definition: hdl_grabber.h:261
pcl::HDLGrabber::sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb
sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb
Definition: hdl_grabber.h:75
pcl::HDLGrabber::fireCurrentSweep
void fireCurrentSweep()
pcl::HDLGrabber
Grabber for the Velodyne High-Definition-Laser (HDL)
Definition: hdl_grabber.h:62
pcl::HDLGrabber::current_sweep_xyzrgba_
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr current_sweep_xyzrgba_
Definition: hdl_grabber.h:255
pcl::Grabber
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:60
pcl::HDLGrabber::sweep_xyzrgba_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba > * sweep_xyzrgba_signal_
Definition: hdl_grabber.h:257
pcl::HDLGrabber::HDLLaserCorrection::sinVertCorrection
double sinVertCorrection
Definition: hdl_grabber.h:245
pcl::HDLGrabber::sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba
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:72
pcl::HDLGrabber::getMaximumNumberOfLasers
virtual std::uint8_t getMaximumNumberOfLasers() const
Returns the maximum number of lasers.
PCL_DEPRECATED
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
Definition: pcl_macros.h:150
pcl::HDLGrabber::scan_xyzrgba_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba > * scan_xyzrgba_signal_
Definition: hdl_grabber.h:260
pcl::HDLGrabber::last_azimuth_
std::uint16_t last_azimuth_
Definition: hdl_grabber.h:252
pcl::HDLGrabber::HDLGrabber
HDLGrabber(const std::string &correctionsFile="", const std::string &pcapFile="")
Constructor taking an optional path to an HDL corrections file.
pcl::HDLGrabber::HDLDataPacket
Definition: hdl_grabber.h:231
pcl::HDLGrabber::sig_cb_velodyne_hdl_sweep_point_cloud_xyzi
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:92
pcl::HDLGrabber::sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba
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:98
pcl::HDLGrabber::HDLDataPacket::mode
std::uint8_t mode
Definition: hdl_grabber.h:234
pcl::RGB
A structure representing RGB color information.
Definition: point_types.hpp:346
pcl::HDLGrabber::sig_cb_velodyne_hdl_scan_point_cloud_xyzi
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:80
pcl::HDLGrabber::HDLLaserCorrection::sinVertOffsetCorrection
double sinVertOffsetCorrection
Definition: hdl_grabber.h:247
pcl::HDLGrabber::HDLLaserCorrection::cosVertOffsetCorrection
double cosVertOffsetCorrection
Definition: hdl_grabber.h:248
pcl::HDLGrabber::HDLLaserCorrection::verticalOffsetCorrection
double verticalOffsetCorrection
Definition: hdl_grabber.h:243
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::uint8_t
std::uint8_t uint8_t
Definition: types.h:54
pcl::HDLGrabber::HDLDataPacket::sensorType
std::uint8_t sensorType
Definition: hdl_grabber.h:235
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::HDLGrabber::HDLLaserCorrection::horizontalOffsetCorrection
double horizontalOffsetCorrection
Definition: hdl_grabber.h:244
pcl::HDLGrabber::setMaximumDistanceThreshold
void setMaximumDistanceThreshold(float &maxThreshold)
Any returns from the HDL with a distance greater than this are discarded.
pcl::HDLGrabber::HDLLaserReturn::distance
std::uint16_t distance
Definition: hdl_grabber.h:218
pcl::HDLGrabber::sig_cb_velodyne_hdl_scan_point_cloud_xyz
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:67
pcl::HDLGrabber::HDLLaserReturn::intensity
std::uint8_t intensity
Definition: hdl_grabber.h:219
pcl::HDLGrabber::HDLLaserCorrection::cosVertCorrection
double cosVertCorrection
Definition: hdl_grabber.h:246
pcl::SynchronizedQueue< std::uint8_t * >
pcl::HDLGrabber::setMinimumDistanceThreshold
void setMinimumDistanceThreshold(float &minThreshold)
Any returns from the HDL with a distance less than this are discarded.
pcl::HDLGrabber::sweep_xyzi_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzi > * sweep_xyzi_signal_
Definition: hdl_grabber.h:258
pcl::HDLGrabber::HDLLaserCorrection::azimuthCorrection
double azimuthCorrection
Definition: hdl_grabber.h:240
pcl::HDLGrabber::scan_xyz_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyz > * scan_xyz_signal_
Definition: hdl_grabber.h:259
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:331
pcl::HDLGrabber::HDLBlock
HDLBlock
Definition: hdl_grabber.h:211
pcl::HDLGrabber::computeXYZI
void computeXYZI(pcl::PointXYZI &pointXYZI, std::uint16_t azimuth, HDLLaserReturn laserReturn, HDLLaserCorrection correction) const
pcl::HDLGrabber::sig_cb_velodyne_hdl_sweep_point_cloud_xyz
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:86