38 #include "pcl/pcl_config.h"
40 #ifndef PCL_IO_HDL_GRABBER_H_
41 #define PCL_IO_HDL_GRABBER_H_
43 #include <pcl/io/grabber.h>
44 #include <pcl/io/impl/synchronized_queue.hpp>
45 #include <pcl/point_types.h>
46 #include <pcl/point_cloud.h>
47 #include <boost/asio.hpp>
50 #define HDL_Grabber_toRadians(x) ((x) * M_PI / 180.0)
65 typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyz) (
66 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
71 typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (
72 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
77 typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (
78 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
79 float startAngle, float);
84 typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (
85 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
90 typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (
91 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
96 typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (
97 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
103 HDLGrabber (
const std::string& correctionsFile =
"",
104 const std::string& pcapFile =
"");
111 HDLGrabber (
const boost::asio::ip::address& ipAddress,
112 const unsigned short port,
const std::string& correctionsFile =
"");
118 virtual
void start ();
121 virtual
void stop ();
126 virtual std::
string getName () const;
131 virtual
bool isRunning () const;
135 virtual
float getFramesPerSecond () const;
140 void filterPackets (const boost::asio::ip::address& ipAddress,
141 const
unsigned short port = 443);
145 void setLaserColorRGB (const pcl::
RGB& color,
unsigned int laserNumber);
151 void setMinimumDistanceThreshold(
float & minThreshold);
157 void setMaximumDistanceThreshold(
float & maxThreshold);
162 float getMinimumDistanceThreshold();
166 float getMaximumDistanceThreshold();
169 static const
int HDL_DATA_PORT = 2368;
170 static const
int HDL_NUM_ROT_ANGLES = 36001;
171 static const
int HDL_LASER_PER_FIRING = 32;
172 static const
int HDL_MAX_NUM_LASERS = 64;
173 static const
int HDL_FIRING_PER_PKT = 12;
174 static const boost::asio::ip::address HDL_DEFAULT_NETWORK_ADDRESS;
178 BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
181 #pragma pack(push, 1)
218 static double *cos_lookup_table_;
219 static double *sin_lookup_table_;
221 boost::asio::ip::udp::endpoint udp_listener_endpoint_;
222 boost::asio::ip::address source_address_filter_;
223 unsigned short source_port_filter_;
224 boost::asio::io_service hdl_read_socket_service_;
225 boost::asio::ip::udp::socket *hdl_read_socket_;
226 std::string pcap_file_name_;
227 boost::thread *queue_consumer_thread_;
228 boost::thread *hdl_read_packet_thread_;
230 bool terminate_read_packet_thread_;
231 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > current_scan_xyz_,
233 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > current_scan_xyzi_,
235 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > current_scan_xyzrgb_,
236 current_sweep_xyzrgb_;
237 unsigned int last_azimuth_;
238 boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
239 boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb>* sweep_xyzrgb_signal_;
240 boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
241 boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
242 boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb>* scan_xyzrgb_signal_;
243 boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
244 pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
245 float min_distance_threshold_;
246 float max_distance_threshold_;
248 void processVelodynePackets ();
249 void enqueueHDLPacket (
const unsigned char *data,
250 std::size_t bytesReceived);
251 void initialize (
const std::string& correctionsFile);
252 void loadCorrectionsFile (
const std::string& correctionsFile);
253 void loadHDL32Corrections ();
254 void readPacketsFromSocket ();
256 void readPacketsFromPcap();
257 #endif //#ifdef HAVE_PCAP
259 void fireCurrentSweep ();
260 void fireCurrentScan (
const unsigned short startAngle,
261 const unsigned short endAngle);
264 bool isAddressUnspecified (
const boost::asio::ip::address& ip_address);
unsigned short blockIdentifier
unsigned short rotationalPosition
unsigned int gpsTimestamp
double sinVertOffsetCorrection
double distanceCorrection
double horizontalOffsetCorrection
double verticalOffsetCorrection
Grabber interface for PCL 1.x device drivers.
A structure representing RGB color information.
Grabber for the Velodyne High-Definition-Laser (HDL)
double verticalCorrection
double cosVertOffsetCorrection