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