Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add support pcl::PointXYZRGBA to pcl::VLPGrabber. Deprecate rgb signatures. #2102

Merged
merged 1 commit into from
Nov 24, 2017
+222 −120
Merged

Add support pcl::PointXYZRGBA to pcl::VLPGrabber. Deprecate rgb signatures. #2102

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
88 changes: 58 additions & 30 deletions io/include/pcl/io/hdl_grabber.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,38 +67,49 @@ namespace pcl
(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);
(sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
float,
float);

typedef PCL_DEPRECATED ("Use 'sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba' instead")
sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb;

/** \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);

/** \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> >&);

/** \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> >&);

/** \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> >&);
(sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);

typedef PCL_DEPRECATED ("Use 'sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba' instead")
sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb;

/** \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
Expand All @@ -113,7 +124,7 @@ namespace pcl
* \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 uint16_t port,
const std::string& correctionsFile = "");

/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
Expand Down Expand Up @@ -150,13 +161,25 @@ namespace pcl
*/
void
filterPackets (const boost::asio::ip::address& ipAddress,
const unsigned short port = 443);
const uint16_t port = 443);

/** \brief Allows one to customize the colors used for each of the lasers.
/** \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,
unsigned int laserNumber);
const 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
Expand All @@ -183,12 +206,17 @@ namespace pcl
float
getMaximumDistanceThreshold ();

/** \brief Returns the maximum number of lasers
*/
virtual uint8_t
getMaximumNumberOfLasers () const;

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 uint16_t HDL_DATA_PORT = 2368;
static const uint16_t HDL_NUM_ROT_ANGLES = 36001;
static const uint8_t HDL_LASER_PER_FIRING = 32;
static const uint8_t HDL_MAX_NUM_LASERS = 64;
static const uint8_t HDL_FIRING_PER_PKT = 12;

enum HDLBlock
{
Expand All @@ -198,24 +226,24 @@ namespace pcl
#pragma pack(push, 1)
typedef struct HDLLaserReturn
{
unsigned short distance;
unsigned char intensity;
uint16_t distance;
uint8_t intensity;
} HDLLaserReturn;
#pragma pack(pop)

struct HDLFiringData
{
unsigned short blockIdentifier;
unsigned short rotationalPosition;
uint16_t blockIdentifier;
uint16_t rotationalPosition;
HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING];
};

struct HDLDataPacket
{
HDLFiringData firingData[HDL_FIRING_PER_PKT];
unsigned int gpsTimestamp;
unsigned char mode;
unsigned char sensorType;
uint32_t gpsTimestamp;
uint8_t mode;
uint8_t sensorType;
};

struct HDLLaserCorrection
Expand All @@ -232,37 +260,37 @@ namespace pcl
};

HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
unsigned int last_azimuth_;
uint16_t 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::shared_ptr<pcl::PointCloud<pcl::PointXYZRGBA> > 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_xyzrgb>* sweep_xyzrgb_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_xyzrgb>* scan_xyzrgb_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 unsigned short startAngle,
const unsigned short endAngle);
fireCurrentScan (const uint16_t startAngle,
const uint16_t endAngle);
void
computeXYZI (pcl::PointXYZI& pointXYZI,
int azimuth,
uint16_t azimuth,
HDLLaserReturn laserReturn,
HDLLaserCorrection correction);


private:
static double *cos_lookup_table_;
static double *sin_lookup_table_;
pcl::SynchronizedQueue<unsigned char *> hdl_data_;
pcl::SynchronizedQueue<uint8_t *> hdl_data_;
boost::asio::ip::udp::endpoint udp_listener_endpoint_;
boost::asio::ip::address source_address_filter_;
unsigned short source_port_filter_;
uint16_t source_port_filter_;
boost::asio::io_service hdl_read_socket_service_;
boost::asio::ip::udp::socket *hdl_read_socket_;
std::string pcap_file_name_;
Expand All @@ -286,7 +314,7 @@ namespace pcl
processVelodynePackets ();

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

void
Expand Down
34 changes: 33 additions & 1 deletion io/include/pcl/io/vlp_grabber.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ namespace pcl
* \param[in] port UDP Port that should be used to listen for VLP packets
*/
VLPGrabber (const boost::asio::ip::address& ipAddress,
const unsigned short port);
const uint16_t port);

/** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
virtual
Expand All @@ -80,13 +80,45 @@ namespace pcl
virtual std::string
getName () const;

/** \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 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 Returns the maximum number of lasers
*/
virtual uint8_t
getMaximumNumberOfLasers () const;

protected:
static const uint8_t VLP_MAX_NUM_LASERS = 16;
static const uint8_t VLP_DUAL_MODE = 0x39;

private:
pcl::RGB laser_rgb_mapping_[VLP_MAX_NUM_LASERS];

virtual void
toPointClouds (HDLDataPacket *dataPacket);

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

void
initializeLaserMapping ();

void
loadVLP16Corrections ();

Expand Down
Loading