41 #include "pcl/pcl_config.h"
44 #include <pcl/io/grabber.h>
45 #include <pcl/io/impl/synchronized_queue.hpp>
47 #include <pcl/point_cloud.h>
48 #include <boost/asio.hpp>
52 #define HDL_Grabber_toRadians(x) ((x) * M_PI / 180.0)
74 using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb
PCL_DEPRECATED(
"use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead")
100 using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb
PCL_DEPRECATED(
"use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead")
107 HDLGrabber (
const std::string& correctionsFile =
"",
108 const std::string& pcapFile =
"");
115 HDLGrabber (
const boost::asio::ip::address& ipAddress,
116 const std::uint16_t port,
117 const std::string& correctionsFile =
"");
135 getName () const override;
141 isRunning () const override;
146 getFramesPerSecond () const override;
152 filterPackets (const
boost::asio::ip::address& ipAddress,
153 const std::uint16_t port = 443);
160 setLaserColorRGB (const
pcl::
RGB& color,
161 const std::uint8_t laserNumber);
167 template<typename IterT>
void
168 setLaserColorRGB (const IterT& begin, const IterT& end)
170 std::copy (begin, end, laser_rgb_mapping_);
178 setMinimumDistanceThreshold (
float & minThreshold);
185 setMaximumDistanceThreshold (
float & maxThreshold);
191 getMinimumDistanceThreshold ();
196 getMaximumDistanceThreshold ();
201 getMaximumNumberOfLasers ()
const;
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;
212 BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
215 #pragma pack(push, 1)
267 fireCurrentScan (
const std::uint16_t startAngle,
268 const std::uint16_t endAngle);
271 std::uint16_t azimuth,
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_;
296 virtual boost::asio::ip::address
297 getDefaultNetworkAddress ();
300 initialize (
const std::string& correctionsFile =
"");
303 processVelodynePackets ();
306 enqueueHDLPacket (
const std::uint8_t *data,
307 std::size_t bytesReceived);
310 loadCorrectionsFile (
const std::string& correctionsFile);
313 loadHDL32Corrections ();
316 readPacketsFromSocket ();
320 readPacketsFromPcap();
322 #endif //#ifdef HAVE_PCAP
325 isAddressUnspecified (
const boost::asio::ip::address& ip_address);