39 #include "pcl/pcl_config.h"
41 #ifndef PCL_IO_HDL_GRABBER_H_
42 #define PCL_IO_HDL_GRABBER_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>
51 #define HDL_Grabber_toRadians(x) ((x) * M_PI / 180.0)
67 (sig_cb_velodyne_hdl_scan_point_cloud_xyz) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
75 (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
79 typedef PCL_DEPRECATED (
"Use 'sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba' instead")
111 typedef PCL_DEPRECATED ("Use 'sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba' instead")
119 const
std::
string& pcapFile = "");
128 const
std::
string& correctionsFile = "");
157 getFramesPerSecond () const;
163 filterPackets (const
boost::asio::ip::address& ipAddress,
164 const uint16_t port = 443);
171 setLaserColorRGB (const
pcl::
RGB& color,
172 const uint8_t laserNumber);
178 template<typename IterT>
void
179 setLaserColorRGB (const IterT& begin, const IterT& end)
181 std::copy (begin, end, laser_rgb_mapping_);
189 setMinimumDistanceThreshold (
float & minThreshold);
196 setMaximumDistanceThreshold (
float & maxThreshold);
202 getMinimumDistanceThreshold ();
207 getMaximumDistanceThreshold ();
212 getMaximumNumberOfLasers ()
const;
215 static const uint16_t HDL_DATA_PORT = 2368;
216 static const uint16_t HDL_NUM_ROT_ANGLES = 36001;
217 static const uint8_t HDL_LASER_PER_FIRING = 32;
218 static const uint8_t HDL_MAX_NUM_LASERS = 64;
219 static const uint8_t HDL_FIRING_PER_PKT = 12;
223 BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
226 #pragma pack(push, 1)
278 fireCurrentScan (
const uint16_t startAngle,
279 const uint16_t endAngle);
288 static double *cos_lookup_table_;
289 static double *sin_lookup_table_;
291 boost::asio::ip::udp::endpoint udp_listener_endpoint_;
292 boost::asio::ip::address source_address_filter_;
293 uint16_t source_port_filter_;
294 boost::asio::io_service hdl_read_socket_service_;
295 boost::asio::ip::udp::socket *hdl_read_socket_;
296 std::string pcap_file_name_;
297 boost::thread *queue_consumer_thread_;
298 boost::thread *hdl_read_packet_thread_;
299 bool terminate_read_packet_thread_;
300 pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
301 float min_distance_threshold_;
302 float max_distance_threshold_;
307 virtual boost::asio::ip::address
308 getDefaultNetworkAddress ();
311 initialize (
const std::string& correctionsFile =
"");
314 processVelodynePackets ();
317 enqueueHDLPacket (
const uint8_t *data,
318 std::size_t bytesReceived);
321 loadCorrectionsFile (
const std::string& correctionsFile);
324 loadHDL32Corrections ();
327 readPacketsFromSocket ();
331 readPacketsFromPcap();
333 #endif //#ifdef HAVE_PCAP
336 isAddressUnspecified (
const boost::asio::ip::address& ip_address);