Skip to content

Commit

Permalink
Added support for Velodyne VLP LiDAR Grabber; Refactored Velodyne HDL…
Browse files Browse the repository at this point in the history 
… Grabber as a result
  • Loading branch information
@KevenRing
KevenRing committed Dec 16, 2015
1 parent 8567baf commit 8e9c67a
Show file tree
Hide file tree
Showing 7 changed files with 830 additions and 223 deletions.
2 changes: 2 additions & 0 deletions io/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -212,6 +212,7 @@ if(build)
src/ifs_io.cpp
src/image_grabber.cpp
src/hdl_grabber.cpp
src/vlp_grabber.cpp
src/robot_eye_grabber.cpp
src/file_io.cpp
src/auto_io.cpp
Expand Down Expand Up @@ -260,6 +261,7 @@ if(build)
"include/pcl/${SUBSYS_NAME}/ifs_io.h"
"include/pcl/${SUBSYS_NAME}/image_grabber.h"
"include/pcl/${SUBSYS_NAME}/hdl_grabber.h"
"include/pcl/${SUBSYS_NAME}/vlp_grabber.h"
"include/pcl/${SUBSYS_NAME}/robot_eye_grabber.h"
"include/pcl/${SUBSYS_NAME}/point_cloud_image_extractors.h"
"include/pcl/${SUBSYS_NAME}/io_exception.h"
Expand Down
173 changes: 108 additions & 65 deletions io/include/pcl/io/hdl_grabber.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,8 @@
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012 The MITRE Corporation
* Copyright (c) 2012-, Open Perception, Inc.
* Copyright (c) 2012,2015 The MITRE Corporation
*
* All rights reserved.
*
Expand Down Expand Up @@ -62,117 +63,132 @@ namespace pcl
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne
*/
typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyz) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
float, float);
typedef void
(sig_cb_velodyne_hdl_scan_point_cloud_xyz) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
float,
float);
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB
*/
typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
float, float);
typedef void
(sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
float,
float);
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
*/
typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
float startAngle, float);
typedef void
(sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
float startAngle,
float);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne
* This signal is sent when the Velodyne passes angle "0"
*/
typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
typedef void
(sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne with the returned intensity
* This signal is sent when the Velodyne passes angle "0"
*/
typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
typedef void
(sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne
* This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB
*/
typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
typedef void
(sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);

/** \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]
* \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
* \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional
*/
HDLGrabber (const std::string& correctionsFile = "",
const std::string& pcapFile = "");
const std::string& pcapFile = "");

/** \brief Constructor taking a pecified IP/port and an optional path to an HDL corrections file.
* \param[in] ipAddress IP Address that should be used to listen for HDL packets
* \param[in] port UDP Port that should be used to listen for HDL packets
* \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
*/
HDLGrabber (const boost::asio::ip::address& ipAddress,
const unsigned short port, const std::string& correctionsFile = "");
const unsigned short port,
const std::string& correctionsFile = "");

/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
virtual ~HDLGrabber () throw ();
virtual
~HDLGrabber () throw ();

/** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */
virtual void start ();
virtual void
start ();

/** \brief Stops processing the Velodyne packets, either from the network or PCAP file */
virtual void stop ();
virtual void
stop ();

/** \brief Obtains the name of this I/O Grabber
* \return The name of the grabber
*/
virtual std::string getName () const;
virtual std::string
getName () const;

/** \brief Check if the grabber is still running.
* \return TRUE if the grabber is running, FALSE otherwise
*/
virtual bool isRunning () const;
virtual bool
isRunning () const;

/** \brief Returns the number of frames per second.
*/
virtual float getFramesPerSecond () const;
virtual float
getFramesPerSecond () const;

/** \brief Allows one to filter packets based on the SOURCE IP address and PORT
* This can be used, for instance, if multiple HDL LIDARs are on the same network
*/
void filterPackets (const boost::asio::ip::address& ipAddress,
const unsigned short port = 443);
void
filterPackets (const boost::asio::ip::address& ipAddress,
const unsigned short port = 443);

/** \brief Allows one to customize the colors used for each of the lasers.
*/
void setLaserColorRGB (const pcl::RGB& color, unsigned int laserNumber);
void
setLaserColorRGB (const pcl::RGB& color,
unsigned int laserNumber);

/** \brief Any returns from the HDL with a distance less than this are discarded.
* This value is in meters
* Default: 0.0
*/
void setMinimumDistanceThreshold(float & minThreshold);
void
setMinimumDistanceThreshold (float & minThreshold);

/** \brief Any returns from the HDL with a distance greater than this are discarded.
* This value is in meters
* Default: 10000.0
*/
void setMaximumDistanceThreshold(float & maxThreshold);
void
setMaximumDistanceThreshold (float & maxThreshold);

/** \brief Returns the current minimum distance threshold, in meters
*/

float getMinimumDistanceThreshold();
float
getMinimumDistanceThreshold ();

/** \brief Returns the current maximum distance threshold, in meters
*/
float getMaximumDistanceThreshold();
float
getMaximumDistanceThreshold ();

protected:
static const int HDL_DATA_PORT = 2368;
static const int HDL_NUM_ROT_ANGLES = 36001;
static const int HDL_LASER_PER_FIRING = 32;
static const int HDL_MAX_NUM_LASERS = 64;
static const int HDL_FIRING_PER_PKT = 12;
static const boost::asio::ip::address HDL_DEFAULT_NETWORK_ADDRESS;

enum HDLBlock
{
BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
Expand All @@ -197,8 +213,8 @@ namespace pcl
{
HDLFiringData firingData[HDL_FIRING_PER_PKT];
unsigned int gpsTimestamp;
unsigned char blank1;
unsigned char blank2;
unsigned char mode;
unsigned char sensorType;
};

struct HDLLaserCorrection
Expand All @@ -214,54 +230,81 @@ namespace pcl
double cosVertOffsetCorrection;
};

void
fireCurrentSweep ();

void
fireCurrentScan (const unsigned short startAngle,
const unsigned short endAngle);
void
computeXYZI (pcl::PointXYZI& pointXYZI,
int azimuth,
HDLLaserReturn laserReturn,
HDLLaserCorrection correction);

HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
unsigned int last_azimuth_;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > current_scan_xyz_, current_sweep_xyz_;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > current_scan_xyzi_, current_sweep_xyzi_;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > current_scan_xyzrgb_, current_sweep_xyzrgb_;
boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb>* sweep_xyzrgb_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb>* scan_xyzrgb_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;

private:
static double *cos_lookup_table_;
static double *sin_lookup_table_;
pcl::SynchronizedQueue<unsigned char *> hdl_data_;
boost::asio::ip::udp::endpoint udp_listener_endpoint_;
boost::asio::ip::address source_address_filter_;
unsigned short source_port_filter_;
boost::asio::io_service hdl_read_socket_service_;
boost::asio::io_service hdl_read_socket_service_;
boost::asio::ip::udp::socket *hdl_read_socket_;
std::string pcap_file_name_;
boost::thread *queue_consumer_thread_;
boost::thread *hdl_read_packet_thread_;
HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
bool terminate_read_packet_thread_;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > current_scan_xyz_,
current_sweep_xyz_;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > current_scan_xyzi_,
current_sweep_xyzi_;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > current_scan_xyzrgb_,
current_sweep_xyzrgb_;
unsigned int last_azimuth_;
boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb>* sweep_xyzrgb_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzi>* sweep_xyzi_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyz>* scan_xyz_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb>* scan_xyzrgb_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
float min_distance_threshold_;
float max_distance_threshold_;

void processVelodynePackets ();
void enqueueHDLPacket (const unsigned char *data,
std::size_t bytesReceived);
void initialize (const std::string& correctionsFile);
void loadCorrectionsFile (const std::string& correctionsFile);
void loadHDL32Corrections ();
void readPacketsFromSocket ();
virtual void
toPointClouds (HDLDataPacket *dataPacket);

virtual boost::asio::ip::address
getDefaultNetworkAddress ();

void
initialize (const std::string& correctionsFile = "");

void
processVelodynePackets ();

void
enqueueHDLPacket (const unsigned char *data,
std::size_t bytesReceived);

void
loadCorrectionsFile (const std::string& correctionsFile);

void
loadHDL32Corrections ();

void
readPacketsFromSocket ();

#ifdef HAVE_PCAP
void readPacketsFromPcap();
void
readPacketsFromPcap();

#endif //#ifdef HAVE_PCAP
void toPointClouds (HDLDataPacket *dataPacket);
void fireCurrentSweep ();
void fireCurrentScan (const unsigned short startAngle,
const unsigned short endAngle);
void computeXYZI (pcl::PointXYZI& pointXYZI, int azimuth,
HDLLaserReturn laserReturn, HDLLaserCorrection correction);
bool isAddressUnspecified (const boost::asio::ip::address& ip_address);

bool
isAddressUnspecified (const boost::asio::ip::address& ip_address);

};
}

Expand Down
Loading

0 comments on commit 8e9c67a

Please sign in to comment.