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> 46 #include <pcl/point_types.h> 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> >&,
74 (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
81 (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
89 (sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
95 (sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
101 (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
107 HDLGrabber (
const std::string& correctionsFile =
"",
108 const std::string& pcapFile =
"");
115 HDLGrabber (
const boost::asio::ip::address& ipAddress,
116 const unsigned short port,
117 const std::string& correctionsFile =
"");
146 getFramesPerSecond () const;
152 filterPackets (const
boost::asio::ip::address& ipAddress,
153 const
unsigned short port = 443);
158 setLaserColorRGB (const
pcl::
RGB& color,
159 unsigned int laserNumber);
166 setMinimumDistanceThreshold (
float & minThreshold);
173 setMaximumDistanceThreshold (
float & maxThreshold);
179 getMinimumDistanceThreshold ();
184 getMaximumDistanceThreshold ();
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;
195 BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
198 #pragma pack(push, 1) 250 fireCurrentScan (
const unsigned short startAngle,
251 const unsigned short endAngle);
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_;
279 virtual boost::asio::ip::address
280 getDefaultNetworkAddress ();
283 initialize (
const std::string& correctionsFile =
"");
286 processVelodynePackets ();
289 enqueueHDLPacket (
const unsigned char *data,
290 std::size_t bytesReceived);
293 loadCorrectionsFile (
const std::string& correctionsFile);
296 loadHDL32Corrections ();
299 readPacketsFromSocket ();
303 readPacketsFromPcap();
305 #endif //#ifdef HAVE_PCAP 308 isAddressUnspecified (
const boost::asio::ip::address& ip_address);
double verticalCorrection
boost::shared_ptr< pcl::PointCloud< pcl::PointXYZRGBA > > current_sweep_xyzrgb_
double horizontalOffsetCorrection
double verticalOffsetCorrection
double distanceCorrection
unsigned int gpsTimestamp
unsigned short blockIdentifier
boost::shared_ptr< pcl::PointCloud< pcl::PointXYZI > > current_sweep_xyzi_
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyz > * scan_xyz_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyz > * sweep_xyz_signal_
double sinVertOffsetCorrection
boost::shared_ptr< pcl::PointCloud< pcl::PointXYZ > > current_sweep_xyz_
Grabber for the Velodyne High-Definition-Laser (HDL)
Grabber interface for PCL 1.x device drivers.
A structure representing RGB color information.
double cosVertOffsetCorrection
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb > * scan_xyzrgb_signal_
unsigned int last_azimuth_
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzi > * scan_xyzi_signal_
unsigned short rotationalPosition
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzi > * sweep_xyzi_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb > * sweep_xyzrgb_signal_