Loading...
Searching...
No Matches
hdl_grabber.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
* Copyright (c) 2012,2015 The MITRE Corporation
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#pragma once
#include "pcl/pcl_config.h"
#include <pcl/pcl_macros.h>
#include <pcl/io/grabber.h>
#include <pcl/io/impl/synchronized_queue.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <boost/asio.hpp>
#include <string>
#include <thread>
#define HDL_Grabber_toRadians(x) ((x) * M_PI / 180.0)
namespace pcl
{
/** \brief Grabber for the Velodyne High-Definition-Laser (HDL)
* \author Keven Ring <keven@mitre.org>
* \ingroup io
*/
class PCL_EXPORTS HDLGrabber : public Grabber
{
public:
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne
*/
using sig_cb_velodyne_hdl_scan_point_cloud_xyz = void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &, float, float);
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB
*/
using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba = void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &, float, float);
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
*/
using sig_cb_velodyne_hdl_scan_point_cloud_xyzi = void (const pcl::PointCloud<pcl::PointXYZI>::ConstPtr &, float, 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"
*/
using sig_cb_velodyne_hdl_sweep_point_cloud_xyz = void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &);
/** \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"
*/
using sig_cb_velodyne_hdl_sweep_point_cloud_xyzi = void (const pcl::PointCloud<pcl::PointXYZI>::ConstPtr &);
/** \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
*/
using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba = void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &);
/** \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 = "");
/** \brief Constructor taking a specified 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 std::uint16_t port,
const std::string& correctionsFile = "");
/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
~HDLGrabber () noexcept override;
/** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */
void
start () override;
/** \brief Stops processing the Velodyne packets, either from the network or PCAP file */
void
stop () override;
/** \brief Obtains the name of this I/O Grabber
* \return The name of the grabber
*/
std::string
getName () const override;
/** \brief Check if the grabber is still running.
* \return TRUE if the grabber is running, FALSE otherwise
*/
bool
isRunning () const override;
/** \brief Returns the number of frames per second.
*/
float
getFramesPerSecond () const override;
/** \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 std::uint16_t port = 443);
/** \brief Allows one to customize the colors used by each laser.
* \param[in] color RGB color to set
* \param[in] laserNumber Number of laser to set color
*/
void
setLaserColorRGB (const pcl::RGB& color,
const std::uint8_t laserNumber);
/** \brief Allows one to customize the colors used for each of the lasers.
* \param[in] begin begin iterator of RGB color array
* \param[in] end end iterator of RGB color array
*/
template<typename IterT> void
setLaserColorRGB (const IterT& begin, const IterT& end)
{
std::copy (begin, end, laser_rgb_mapping_);
}
/** \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);
/** \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);
/** \brief Returns the current minimum distance threshold, in meters
*/
float
getMinimumDistanceThreshold ();
/** \brief Returns the current maximum distance threshold, in meters
*/
float
getMaximumDistanceThreshold ();
/** \brief Returns the maximum number of lasers
*/
virtual std::uint8_t
getMaximumNumberOfLasers () const;
protected:
static const std::uint16_t HDL_DATA_PORT = 2368;
static const std::uint16_t HDL_NUM_ROT_ANGLES = 36001;
static const std::uint8_t HDL_LASER_PER_FIRING = 32;
static const std::uint8_t HDL_MAX_NUM_LASERS = 64;
static const std::uint8_t HDL_FIRING_PER_PKT = 12;
enum HDLBlock
{
BLOCK_0_TO_31 = 0xeeff, BLOCK_32_TO_63 = 0xddff
};
#pragma pack(push, 1)
struct HDLLaserReturn
{
std::uint16_t distance;
std::uint8_t intensity;
};
#pragma pack(pop)
struct HDLFiringData
{
std::uint16_t blockIdentifier;
std::uint16_t rotationalPosition;
HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING];
};
struct HDLDataPacket
{
HDLFiringData firingData[HDL_FIRING_PER_PKT];
std::uint32_t gpsTimestamp;
std::uint8_t mode;
std::uint8_t sensorType;
};
struct HDLLaserCorrection
{
double azimuthCorrection;
double verticalCorrection;
double distanceCorrection;
double verticalOffsetCorrection;
double horizontalOffsetCorrection;
double sinVertCorrection;
double cosVertCorrection;
double sinVertOffsetCorrection;
double cosVertOffsetCorrection;
};
HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
std::uint16_t last_azimuth_;
pcl::PointCloud<pcl::PointXYZ>::Ptr current_scan_xyz_, current_sweep_xyz_;
pcl::PointCloud<pcl::PointXYZI>::Ptr current_scan_xyzi_, current_sweep_xyzi_;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr current_scan_xyzrgba_, current_sweep_xyzrgba_;
boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyz>* sweep_xyz_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba>* sweep_xyzrgba_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_xyzrgba>* scan_xyzrgba_signal_;
boost::signals2::signal<sig_cb_velodyne_hdl_scan_point_cloud_xyzi>* scan_xyzi_signal_;
void
fireCurrentSweep ();
void
fireCurrentScan (const std::uint16_t startAngle,
const std::uint16_t endAngle);
void
computeXYZI (pcl::PointXYZI& pointXYZI,
std::uint16_t azimuth,
HDLLaserReturn laserReturn,
HDLLaserCorrection correction) const;
private:
static double *cos_lookup_table_;
static double *sin_lookup_table_;
pcl::SynchronizedQueue<std::uint8_t *> hdl_data_;
boost::asio::ip::udp::endpoint udp_listener_endpoint_;
boost::asio::ip::address source_address_filter_;
std::uint16_t source_port_filter_;
boost::asio::io_context hdl_read_socket_service_;
boost::asio::ip::udp::socket *hdl_read_socket_;
std::string pcap_file_name_;
std::thread *queue_consumer_thread_;
std::thread *hdl_read_packet_thread_;
bool terminate_read_packet_thread_;
pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS];
float min_distance_threshold_;
float max_distance_threshold_;
virtual void
toPointClouds (HDLDataPacket *dataPacket);
virtual boost::asio::ip::address
getDefaultNetworkAddress ();
void
initialize (const std::string& correctionsFile = "");
void
processVelodynePackets ();
void
enqueueHDLPacket (const std::uint8_t *data,
std::size_t bytesReceived);
void
loadCorrectionsFile (const std::string& correctionsFile);
void
loadHDL32Corrections ();
void
readPacketsFromSocket ();
#ifdef HAVE_PCAP
void
readPacketsFromPcap();
#endif //#ifdef HAVE_PCAP
bool
isAddressUnspecified (const boost::asio::ip::address& ip_address);
};
}
pcl::Grabber
Grabber interface for PCL 1.x device drivers.
Definition grabber.h:60
pcl::HDLGrabber
Grabber for the Velodyne High-Definition-Laser (HDL)
Definition hdl_grabber.h:62
pcl::HDLGrabber::scan_xyz_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyz > * scan_xyz_signal_
Definition hdl_grabber.h:253
pcl::HDLGrabber::HDLBlock
HDLBlock
Definition hdl_grabber.h:205
pcl::HDLGrabber::sig_cb_velodyne_hdl_scan_point_cloud_xyzi
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &, float, float) sig_cb_velodyne_hdl_scan_point_cloud_xyzi
Signal used for a single sector Represents 1 corrected packet from the HDL Velodyne with the returned...
Definition hdl_grabber.h:77
pcl::HDLGrabber::HDLGrabber
HDLGrabber(const boost::asio::ip::address &ipAddress, const std::uint16_t port, const std::string &correctionsFile="")
Constructor taking a specified IP/port and an optional path to an HDL corrections file.
pcl::HDLGrabber::setMinimumDistanceThreshold
void setMinimumDistanceThreshold(float &minThreshold)
Any returns from the HDL with a distance less than this are discarded.
pcl::HDLGrabber::current_scan_xyzrgba_
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr current_scan_xyzrgba_
Definition hdl_grabber.h:249
pcl::HDLGrabber::fireCurrentSweep
void fireCurrentSweep()
pcl::HDLGrabber::sweep_xyz_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyz > * sweep_xyz_signal_
Definition hdl_grabber.h:250
pcl::HDLGrabber::~HDLGrabber
~HDLGrabber() noexcept override
virtual Destructor inherited from the Grabber interface.
pcl::HDLGrabber::current_scan_xyzi_
pcl::PointCloud< pcl::PointXYZI >::Ptr current_scan_xyzi_
Definition hdl_grabber.h:248
pcl::HDLGrabber::getMaximumNumberOfLasers
virtual std::uint8_t getMaximumNumberOfLasers() const
Returns the maximum number of lasers.
pcl::HDLGrabber::sweep_xyzrgba_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba > * sweep_xyzrgba_signal_
Definition hdl_grabber.h:251
pcl::HDLGrabber::computeXYZI
void computeXYZI(pcl::PointXYZI &pointXYZI, std::uint16_t azimuth, HDLLaserReturn laserReturn, HDLLaserCorrection correction) const
pcl::HDLGrabber::setMaximumDistanceThreshold
void setMaximumDistanceThreshold(float &maxThreshold)
Any returns from the HDL with a distance greater than this are discarded.
pcl::HDLGrabber::sweep_xyzi_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_sweep_point_cloud_xyzi > * sweep_xyzi_signal_
Definition hdl_grabber.h:252
pcl::HDLGrabber::getMinimumDistanceThreshold
float getMinimumDistanceThreshold()
Returns the current minimum distance threshold, in meters.
pcl::HDLGrabber::sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &) sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba
Signal used for a 360 degree sweep Represents multiple corrected packets from the HDL Velodyne This s...
Definition hdl_grabber.h:95
pcl::HDLGrabber::getMaximumDistanceThreshold
float getMaximumDistanceThreshold()
Returns the current maximum distance threshold, in meters.
pcl::HDLGrabber::sig_cb_velodyne_hdl_sweep_point_cloud_xyz
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &) sig_cb_velodyne_hdl_sweep_point_cloud_xyz
Signal used for a 360 degree sweep Represents multiple corrected packets from the HDL Velodyne This s...
Definition hdl_grabber.h:83
pcl::HDLGrabber::HDLGrabber
HDLGrabber(const std::string &correctionsFile="", const std::string &pcapFile="")
Constructor taking an optional path to an HDL corrections file.
pcl::HDLGrabber::sig_cb_velodyne_hdl_sweep_point_cloud_xyzi
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &) sig_cb_velodyne_hdl_sweep_point_cloud_xyzi
Signal used for a 360 degree sweep Represents multiple corrected packets from the HDL Velodyne with t...
Definition hdl_grabber.h:89
pcl::HDLGrabber::fireCurrentScan
void fireCurrentScan(const std::uint16_t startAngle, const std::uint16_t endAngle)
pcl::HDLGrabber::sig_cb_velodyne_hdl_scan_point_cloud_xyz
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &, float, float) sig_cb_velodyne_hdl_scan_point_cloud_xyz
Signal used for a single sector Represents 1 corrected packet from the HDL Velodyne.
Definition hdl_grabber.h:67
pcl::HDLGrabber::current_scan_xyz_
pcl::PointCloud< pcl::PointXYZ >::Ptr current_scan_xyz_
Definition hdl_grabber.h:247
pcl::HDLGrabber::last_azimuth_
std::uint16_t last_azimuth_
Definition hdl_grabber.h:246
pcl::HDLGrabber::sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &, float, float) sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba
Signal used for a single sector Represents 1 corrected packet from the HDL Velodyne.
Definition hdl_grabber.h:72
pcl::HDLGrabber::scan_xyzrgba_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba > * scan_xyzrgba_signal_
Definition hdl_grabber.h:254
pcl::HDLGrabber::scan_xyzi_signal_
boost::signals2::signal< sig_cb_velodyne_hdl_scan_point_cloud_xyzi > * scan_xyzi_signal_
Definition hdl_grabber.h:255
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
pcl::SynchronizedQueue
Definition synchronized_queue.hpp:52
point_types.h
Defines all the PCL implemented PointT point type structures.
boost
Definition boost_graph.h:48
pcl
Definition convolution.h:46
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::HDLGrabber::HDLDataPacket
Definition hdl_grabber.h:225
pcl::HDLGrabber::HDLDataPacket::mode
std::uint8_t mode
Definition hdl_grabber.h:228
pcl::HDLGrabber::HDLDataPacket::gpsTimestamp
std::uint32_t gpsTimestamp
Definition hdl_grabber.h:227
pcl::HDLGrabber::HDLDataPacket::sensorType
std::uint8_t sensorType
Definition hdl_grabber.h:229
pcl::HDLGrabber::HDLFiringData
Definition hdl_grabber.h:218
pcl::HDLGrabber::HDLFiringData::blockIdentifier
std::uint16_t blockIdentifier
Definition hdl_grabber.h:219
pcl::HDLGrabber::HDLFiringData::rotationalPosition
std::uint16_t rotationalPosition
Definition hdl_grabber.h:220
pcl::HDLGrabber::HDLLaserCorrection
Definition hdl_grabber.h:233
pcl::HDLGrabber::HDLLaserCorrection::cosVertOffsetCorrection
double cosVertOffsetCorrection
Definition hdl_grabber.h:242
pcl::HDLGrabber::HDLLaserCorrection::azimuthCorrection
double azimuthCorrection
Definition hdl_grabber.h:234
pcl::HDLGrabber::HDLLaserCorrection::distanceCorrection
double distanceCorrection
Definition hdl_grabber.h:236
pcl::HDLGrabber::HDLLaserCorrection::sinVertCorrection
double sinVertCorrection
Definition hdl_grabber.h:239
pcl::HDLGrabber::HDLLaserCorrection::horizontalOffsetCorrection
double horizontalOffsetCorrection
Definition hdl_grabber.h:238
pcl::HDLGrabber::HDLLaserCorrection::sinVertOffsetCorrection
double sinVertOffsetCorrection
Definition hdl_grabber.h:241
pcl::HDLGrabber::HDLLaserCorrection::verticalOffsetCorrection
double verticalOffsetCorrection
Definition hdl_grabber.h:237
pcl::HDLGrabber::HDLLaserCorrection::cosVertCorrection
double cosVertCorrection
Definition hdl_grabber.h:240
pcl::HDLGrabber::HDLLaserCorrection::verticalCorrection
double verticalCorrection
Definition hdl_grabber.h:235
pcl::HDLGrabber::HDLLaserReturn
Definition hdl_grabber.h:211
pcl::HDLGrabber::HDLLaserReturn::intensity
std::uint8_t intensity
Definition hdl_grabber.h:213
pcl::HDLGrabber::HDLLaserReturn::distance
std::uint16_t distance
Definition hdl_grabber.h:212
pcl::PointXYZI
Definition point_types.hpp:457
pcl::RGB
A structure representing RGB color information.
Definition point_types.hpp:363