From 67f69bc31dcd32da10b69c2efc77a5333140cbbe Mon Sep 17 00:00:00 2001 From: Tsukasa Sugiura Date: Fri, 24 Nov 2017 22:06:42 +0900 Subject: [PATCH] Add support pcl::PointXYZRGBA to pcl::VLPGrabber Add support pcl::PointXYZRGBA to pcl::VLPGrabber. This change will be able to attach color to point cloud using pcl::visualization::PointCloudColorHandlerRGBField. --- io/include/pcl/io/hdl_grabber.h | 88 ++++++++++++------- io/include/pcl/io/vlp_grabber.h | 34 +++++++- io/src/hdl_grabber.cpp | 145 +++++++++++++++++--------------- io/src/vlp_grabber.cpp | 75 ++++++++++++----- 4 files changed, 222 insertions(+), 120 deletions(-) diff --git a/io/include/pcl/io/hdl_grabber.h b/io/include/pcl/io/hdl_grabber.h index 6817eb58684..28dacf95ea3 100644 --- a/io/include/pcl/io/hdl_grabber.h +++ b/io/include/pcl/io/hdl_grabber.h @@ -67,13 +67,18 @@ namespace pcl (sig_cb_velodyne_hdl_scan_point_cloud_xyz) (const boost::shared_ptr >&, 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 >&, - float, - float); + (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba) (const boost::shared_ptr >&, + 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. */ @@ -81,24 +86,30 @@ namespace pcl (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (const boost::shared_ptr >&, 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 >&); + /** \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 >&); + /** \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 >&); + (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba) (const boost::shared_ptr >&); + + 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 @@ -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. */ @@ -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 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 @@ -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 { @@ -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 @@ -232,26 +260,26 @@ namespace pcl }; HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS]; - unsigned int last_azimuth_; + uint16_t last_azimuth_; boost::shared_ptr > current_scan_xyz_, current_sweep_xyz_; boost::shared_ptr > current_scan_xyzi_, current_sweep_xyzi_; - boost::shared_ptr > current_scan_xyzrgb_, current_sweep_xyzrgb_; + boost::shared_ptr > current_scan_xyzrgba_, current_sweep_xyzrgba_; boost::signals2::signal* sweep_xyz_signal_; - boost::signals2::signal* sweep_xyzrgb_signal_; + boost::signals2::signal* sweep_xyzrgba_signal_; boost::signals2::signal* sweep_xyzi_signal_; boost::signals2::signal* scan_xyz_signal_; - boost::signals2::signal* scan_xyzrgb_signal_; + boost::signals2::signal* scan_xyzrgba_signal_; boost::signals2::signal* 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); @@ -259,10 +287,10 @@ namespace pcl private: static double *cos_lookup_table_; static double *sin_lookup_table_; - pcl::SynchronizedQueue hdl_data_; + pcl::SynchronizedQueue 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_; @@ -286,7 +314,7 @@ namespace pcl processVelodynePackets (); void - enqueueHDLPacket (const unsigned char *data, + enqueueHDLPacket (const uint8_t *data, std::size_t bytesReceived); void diff --git a/io/include/pcl/io/vlp_grabber.h b/io/include/pcl/io/vlp_grabber.h index 123f9bcbf47..64ee63dcff3 100644 --- a/io/include/pcl/io/vlp_grabber.h +++ b/io/include/pcl/io/vlp_grabber.h @@ -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 @@ -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 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 (); diff --git a/io/src/hdl_grabber.cpp b/io/src/hdl_grabber.cpp index bc57f16531c..c1bdb62a9dc 100644 --- a/io/src/hdl_grabber.cpp +++ b/io/src/hdl_grabber.cpp @@ -63,13 +63,13 @@ pcl::HDLGrabber::HDLGrabber (const std::string& correctionsFile, current_sweep_xyz_ (new pcl::PointCloud ()), current_scan_xyzi_ (new pcl::PointCloud ()), current_sweep_xyzi_ (new pcl::PointCloud ()), - current_scan_xyzrgb_ (new pcl::PointCloud ()), - current_sweep_xyzrgb_ (new pcl::PointCloud ()), + current_scan_xyzrgba_ (new pcl::PointCloud ()), + current_sweep_xyzrgba_ (new pcl::PointCloud ()), sweep_xyz_signal_ (), - sweep_xyzrgb_signal_ (), + sweep_xyzrgba_signal_ (), sweep_xyzi_signal_ (), scan_xyz_signal_ (), - scan_xyzrgb_signal_ (), + scan_xyzrgba_signal_ (), scan_xyzi_signal_ (), hdl_data_ (), udp_listener_endpoint_ (), @@ -88,20 +88,20 @@ pcl::HDLGrabber::HDLGrabber (const std::string& correctionsFile, ///////////////////////////////////////////////////////////////////////////// pcl::HDLGrabber::HDLGrabber (const boost::asio::ip::address& ipAddress, - const unsigned short int port, + const uint16_t port, const std::string& correctionsFile) : last_azimuth_ (65000), current_scan_xyz_ (new pcl::PointCloud ()), current_sweep_xyz_ (new pcl::PointCloud ()), current_scan_xyzi_ (new pcl::PointCloud ()), current_sweep_xyzi_ (new pcl::PointCloud ()), - current_scan_xyzrgb_ (new pcl::PointCloud ()), - current_sweep_xyzrgb_ (new pcl::PointCloud ()), + current_scan_xyzrgba_ (new pcl::PointCloud ()), + current_sweep_xyzrgba_ (new pcl::PointCloud ()), sweep_xyz_signal_ (), - sweep_xyzrgb_signal_ (), + sweep_xyzrgba_signal_ (), sweep_xyzi_signal_ (), scan_xyz_signal_ (), - scan_xyzrgb_signal_ (), + scan_xyzrgba_signal_ (), scan_xyzi_signal_ (), hdl_data_ (), udp_listener_endpoint_ (ipAddress, port), @@ -124,10 +124,10 @@ pcl::HDLGrabber::~HDLGrabber () throw () stop (); disconnect_all_slots (); - disconnect_all_slots (); + disconnect_all_slots (); disconnect_all_slots (); disconnect_all_slots (); - disconnect_all_slots (); + disconnect_all_slots (); disconnect_all_slots (); } @@ -139,7 +139,7 @@ pcl::HDLGrabber::initialize (const std::string& correctionsFile) { cos_lookup_table_ = static_cast (malloc (HDL_NUM_ROT_ANGLES * sizeof (*cos_lookup_table_))); sin_lookup_table_ = static_cast (malloc (HDL_NUM_ROT_ANGLES * sizeof (*sin_lookup_table_))); - for (int i = 0; i < HDL_NUM_ROT_ANGLES; i++) + for (uint16_t i = 0; i < HDL_NUM_ROT_ANGLES; i++) { double rad = (M_PI / 180.0) * (static_cast (i) / 100.0); cos_lookup_table_[i] = std::cos (rad); @@ -149,17 +149,17 @@ pcl::HDLGrabber::initialize (const std::string& correctionsFile) loadCorrectionsFile (correctionsFile); - for (int i = 0; i < HDL_MAX_NUM_LASERS; i++) + for (uint8_t i = 0; i < HDL_MAX_NUM_LASERS; i++) { HDLLaserCorrection correction = laser_corrections_[i]; laser_corrections_[i].sinVertOffsetCorrection = correction.verticalOffsetCorrection * correction.sinVertCorrection; laser_corrections_[i].cosVertOffsetCorrection = correction.verticalOffsetCorrection * correction.cosVertCorrection; } sweep_xyz_signal_ = createSignal (); - sweep_xyzrgb_signal_ = createSignal (); + sweep_xyzrgba_signal_ = createSignal (); sweep_xyzi_signal_ = createSignal (); scan_xyz_signal_ = createSignal (); - scan_xyzrgb_signal_ = createSignal (); + scan_xyzrgba_signal_ = createSignal (); scan_xyzi_signal_ = createSignal (); current_scan_xyz_.reset (new pcl::PointCloud); @@ -167,12 +167,12 @@ pcl::HDLGrabber::initialize (const std::string& correctionsFile) current_sweep_xyz_.reset (new pcl::PointCloud); current_sweep_xyzi_.reset (new pcl::PointCloud); - for (int i = 0; i < HDL_MAX_NUM_LASERS; i++) + for (uint8_t i = 0; i < HDL_MAX_NUM_LASERS; i++) laser_rgb_mapping_[i].r = laser_rgb_mapping_[i].g = laser_rgb_mapping_[i].b = 0; if (laser_corrections_[32].distanceCorrection == 0.0) { - for (int i = 0; i < 16; i++) + for (uint8_t i = 0; i < 16; i++) { laser_rgb_mapping_[i * 2].b = static_cast (i * 6 + 64); laser_rgb_mapping_[i * 2 + 1].b = static_cast ( (i + 16) * 6 + 64); @@ -180,12 +180,12 @@ pcl::HDLGrabber::initialize (const std::string& correctionsFile) } else { - for (int i = 0; i < 16; i++) + for (uint8_t i = 0; i < 16; i++) { laser_rgb_mapping_[i * 2].b = static_cast (i * 3 + 64); laser_rgb_mapping_[i * 2 + 1].b = static_cast ( (i + 16) * 3 + 64); } - for (int i = 0; i < 16; i++) + for (uint8_t i = 0; i < 16; i++) { laser_rgb_mapping_[i * 2 + 32].b = static_cast (i * 3 + 160); laser_rgb_mapping_[i * 2 + 33].b = static_cast ( (i + 16) * 3 + 160); @@ -224,7 +224,7 @@ pcl::HDLGrabber::loadCorrectionsFile (const std::string& correctionsFile) if (px.first == "px") { boost::property_tree::ptree calibration_data = px.second; - int index = -1; + int32_t index = -1; double azimuth = 0, vert_correction = 0, dist_correction = 0, vert_offset_correction = 0, horiz_offset_correction = 0; BOOST_FOREACH (boost::property_tree::ptree::value_type &item, calibration_data) @@ -265,7 +265,7 @@ pcl::HDLGrabber::loadHDL32Corrections () { double hdl32_vertical_corrections[] = { -30.67, -9.3299999, -29.33, -8, -28, -6.6700001, -26.67, -5.3299999, -25.33, -4, -24, -2.6700001, -22.67, -1.33, -21.33, 0, -20, 1.33, -18.67, 2.6700001, -17.33, 4, -16, 5.3299999, -14.67, 6.6700001, -13.33, 8, -12, 9.3299999, -10.67, 10.67 }; - for (int i = 0; i < HDL_LASER_PER_FIRING; i++) + for (uint8_t i = 0; i < HDL_LASER_PER_FIRING; i++) { laser_corrections_[i].azimuthCorrection = 0.0; laser_corrections_[i].distanceCorrection = 0.0; @@ -275,7 +275,7 @@ pcl::HDLGrabber::loadHDL32Corrections () laser_corrections_[i].sinVertCorrection = std::sin (HDL_Grabber_toRadians(hdl32_vertical_corrections[i])); laser_corrections_[i].cosVertCorrection = std::cos (HDL_Grabber_toRadians(hdl32_vertical_corrections[i])); } - for (int i = HDL_LASER_PER_FIRING; i < HDL_MAX_NUM_LASERS; i++) + for (uint8_t i = HDL_LASER_PER_FIRING; i < HDL_MAX_NUM_LASERS; i++) { laser_corrections_[i].azimuthCorrection = 0.0; laser_corrections_[i].distanceCorrection = 0.0; @@ -300,7 +300,7 @@ pcl::HDLGrabber::processVelodynePackets () { while (true) { - unsigned char *data; + uint8_t *data; if (!hdl_data_.dequeue (data)) return; @@ -320,7 +320,7 @@ pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket) return; current_scan_xyz_.reset (new pcl::PointCloud ()); - current_scan_xyzrgb_.reset (new pcl::PointCloud ()); + current_scan_xyzrgba_.reset (new pcl::PointCloud ()); current_scan_xyzi_.reset (new pcl::PointCloud ()); time_t system_time; @@ -328,30 +328,30 @@ pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket) time_t velodyne_time = (system_time & 0x00000000ffffffffl) << 32 | dataPacket->gpsTimestamp; current_scan_xyz_->header.stamp = velodyne_time; - current_scan_xyzrgb_->header.stamp = velodyne_time; + current_scan_xyzrgba_->header.stamp = velodyne_time; current_scan_xyzi_->header.stamp = velodyne_time; current_scan_xyz_->header.seq = scan_counter; - current_scan_xyzrgb_->header.seq = scan_counter; + current_scan_xyzrgba_->header.seq = scan_counter; current_scan_xyzi_->header.seq = scan_counter; scan_counter++; - for (int i = 0; i < HDL_FIRING_PER_PKT; ++i) + for (uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i) { HDLFiringData firing_data = dataPacket->firingData[i]; - int offset = (firing_data.blockIdentifier == BLOCK_0_TO_31) ? 0 : 32; + uint8_t offset = (firing_data.blockIdentifier == BLOCK_0_TO_31) ? 0 : 32; - for (int j = 0; j < HDL_LASER_PER_FIRING; j++) + for (uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++) { if (firing_data.rotationalPosition < last_azimuth_) { - if (current_sweep_xyzrgb_->size () > 0) + if (current_sweep_xyzrgba_->size () > 0) { - current_sweep_xyz_->is_dense = current_sweep_xyzrgb_->is_dense = current_sweep_xyzi_->is_dense = false; + current_sweep_xyz_->is_dense = current_sweep_xyzrgba_->is_dense = current_sweep_xyzi_->is_dense = false; current_sweep_xyz_->header.stamp = velodyne_time; - current_sweep_xyzrgb_->header.stamp = velodyne_time; + current_sweep_xyzrgba_->header.stamp = velodyne_time; current_sweep_xyzi_->header.stamp = velodyne_time; current_sweep_xyz_->header.seq = sweep_counter; - current_sweep_xyzrgb_->header.seq = sweep_counter; + current_sweep_xyzrgba_->header.seq = sweep_counter; current_sweep_xyzi_->header.seq = sweep_counter; sweep_counter++; @@ -359,21 +359,21 @@ pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket) fireCurrentSweep (); } current_sweep_xyz_.reset (new pcl::PointCloud ()); - current_sweep_xyzrgb_.reset (new pcl::PointCloud ()); + current_sweep_xyzrgba_.reset (new pcl::PointCloud ()); current_sweep_xyzi_.reset (new pcl::PointCloud ()); } PointXYZ xyz; PointXYZI xyzi; - PointXYZRGBA xyzrgb; + PointXYZRGBA xyzrgba; computeXYZI (xyzi, firing_data.rotationalPosition, firing_data.laserReturns[j], laser_corrections_[j + offset]); - xyz.x = xyzrgb.x = xyzi.x; - xyz.y = xyzrgb.y = xyzi.y; - xyz.z = xyzrgb.z = xyzi.z; + xyz.x = xyzrgba.x = xyzi.x; + xyz.y = xyzrgba.y = xyzi.y; + xyz.z = xyzrgba.z = xyzi.z; - xyzrgb.rgba = laser_rgb_mapping_[j + offset].rgba; + xyzrgba.rgba = laser_rgb_mapping_[j + offset].rgba; if (pcl_isnan (xyz.x) || pcl_isnan (xyz.y) || pcl_isnan (xyz.z)) { continue; @@ -381,24 +381,24 @@ pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket) current_scan_xyz_->push_back (xyz); current_scan_xyzi_->push_back (xyzi); - current_scan_xyzrgb_->push_back (xyzrgb); + current_scan_xyzrgba_->push_back (xyzrgba); current_sweep_xyz_->push_back (xyz); current_sweep_xyzi_->push_back (xyzi); - current_sweep_xyzrgb_->push_back (xyzrgb); + current_sweep_xyzrgba_->push_back (xyzrgba); last_azimuth_ = firing_data.rotationalPosition; } } - current_scan_xyz_->is_dense = current_scan_xyzrgb_->is_dense = current_scan_xyzi_->is_dense = true; + current_scan_xyz_->is_dense = current_scan_xyzrgba_->is_dense = current_scan_xyzi_->is_dense = true; fireCurrentScan (dataPacket->firingData[0].rotationalPosition, dataPacket->firingData[11].rotationalPosition); } ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point, - int azimuth, + uint16_t azimuth, HDLLaserReturn laserReturn, HDLLaserCorrection correction) { @@ -445,8 +445,8 @@ pcl::HDLGrabber::fireCurrentSweep () if (sweep_xyz_signal_ != NULL && sweep_xyz_signal_->num_slots () > 0) sweep_xyz_signal_->operator() (current_sweep_xyz_); - if (sweep_xyzrgb_signal_ != NULL && sweep_xyzrgb_signal_->num_slots () > 0) - sweep_xyzrgb_signal_->operator() (current_sweep_xyzrgb_); + if (sweep_xyzrgba_signal_ != NULL && sweep_xyzrgba_signal_->num_slots () > 0) + sweep_xyzrgba_signal_->operator() (current_sweep_xyzrgba_); if (sweep_xyzi_signal_ != NULL && sweep_xyzi_signal_->num_slots () > 0) sweep_xyzi_signal_->operator() (current_sweep_xyzi_); @@ -454,8 +454,8 @@ pcl::HDLGrabber::fireCurrentSweep () ///////////////////////////////////////////////////////////////////////////// void -pcl::HDLGrabber::fireCurrentScan (const unsigned short startAngle, - const unsigned short endAngle) +pcl::HDLGrabber::fireCurrentScan (const uint16_t startAngle, + const uint16_t endAngle) { const float start = static_cast (startAngle) / 100.0f; const float end = static_cast (endAngle) / 100.0f; @@ -463,8 +463,8 @@ pcl::HDLGrabber::fireCurrentScan (const unsigned short startAngle, if (scan_xyz_signal_->num_slots () > 0) scan_xyz_signal_->operator () (current_scan_xyz_, start, end); - if (scan_xyzrgb_signal_->num_slots () > 0) - scan_xyzrgb_signal_->operator () (current_scan_xyzrgb_, start, end); + if (scan_xyzrgba_signal_->num_slots () > 0) + scan_xyzrgba_signal_->operator () (current_scan_xyzrgba_, start, end); if (scan_xyzi_signal_->num_slots () > 0) scan_xyzi_signal_->operator() (current_scan_xyzi_, start, end); @@ -472,13 +472,13 @@ pcl::HDLGrabber::fireCurrentScan (const unsigned short startAngle, ///////////////////////////////////////////////////////////////////////////// void -pcl::HDLGrabber::enqueueHDLPacket (const unsigned char *data, +pcl::HDLGrabber::enqueueHDLPacket (const uint8_t *data, std::size_t bytesReceived) { if (bytesReceived == 1206) { - unsigned char *dup = static_cast (malloc (bytesReceived * sizeof(unsigned char))); - memcpy (dup, data, bytesReceived * sizeof(unsigned char)); + uint8_t *dup = static_cast (malloc (bytesReceived * sizeof(uint8_t))); + memcpy (dup, data, bytesReceived * sizeof (uint8_t)); hdl_data_.enqueue (dup); } @@ -528,7 +528,7 @@ pcl::HDLGrabber::start () else { #ifdef HAVE_PCAP - hdl_read_packet_thread_ = new boost::thread(boost::bind(&HDLGrabber::readPacketsFromPcap, this)); + hdl_read_packet_thread_ = new boost::thread (boost::bind (&HDLGrabber::readPacketsFromPcap, this)); #endif // #ifdef HAVE_PCAP } } @@ -585,7 +585,7 @@ pcl::HDLGrabber::getFramesPerSecond () const ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::filterPackets (const boost::asio::ip::address& ipAddress, - const unsigned short port) + const uint16_t port) { source_address_filter_ = ipAddress; source_port_filter_ = port; @@ -594,9 +594,9 @@ pcl::HDLGrabber::filterPackets (const boost::asio::ip::address& ipAddress, ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::setLaserColorRGB (const pcl::RGB& color, - unsigned int laserNumber) + const uint8_t laserNumber) { - if (laserNumber >= (unsigned int) HDL_MAX_NUM_LASERS) + if (laserNumber >= HDL_MAX_NUM_LASERS) return; laser_rgb_mapping_[laserNumber] = color; @@ -610,7 +610,7 @@ pcl::HDLGrabber::isAddressUnspecified (const boost::asio::ip::address& ipAddress return (ipAddress.is_unspecified ()); #else if (ipAddress.is_v4 ()) - return (ipAddress.to_v4 ().to_ulong() == 0); + return (ipAddress.to_v4 ().to_ulong () == 0); return (false); #endif @@ -644,11 +644,18 @@ pcl::HDLGrabber::getMinimumDistanceThreshold () return (min_distance_threshold_); } +///////////////////////////////////////////////////////////////////////////// +uint8_t +pcl::HDLGrabber::getMaximumNumberOfLasers () const +{ + return (HDL_MAX_NUM_LASERS); +} + ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::readPacketsFromSocket () { - unsigned char data[1500]; + uint8_t data[1500]; udp::endpoint sender_endpoint; while (!terminate_read_packet_thread_ && hdl_read_socket_->is_open ()) @@ -669,22 +676,22 @@ void pcl::HDLGrabber::readPacketsFromPcap () { struct pcap_pkthdr *header; - const unsigned char *data; - char errbuff[PCAP_ERRBUF_SIZE]; + const uint8_t *data; + int8_t errbuff[PCAP_ERRBUF_SIZE]; - pcap_t *pcap = pcap_open_offline (pcap_file_name_.c_str (), errbuff); + pcap_t *pcap = pcap_open_offline (pcap_file_name_.c_str (), reinterpret_cast (errbuff)); struct bpf_program filter; std::ostringstream string_stream; string_stream << "udp "; - if (!isAddressUnspecified(source_address_filter_)) + if (!isAddressUnspecified (source_address_filter_)) { - string_stream << " and src port " << source_port_filter_ << " and src host " << source_address_filter_.to_string(); + string_stream << " and src port " << source_port_filter_ << " and src host " << source_address_filter_.to_string (); } // PCAP_NETMASK_UNKNOWN should be 0xffffffff, but it's undefined in older PCAP versions - if (pcap_compile (pcap, &filter, string_stream.str ().c_str(), 0, 0xffffffff) == -1) + if (pcap_compile (pcap, &filter, string_stream.str ().c_str (), 0, 0xffffffff) == -1) { PCL_WARN ("[pcl::HDLGrabber::readPacketsFromPcap] Issue compiling filter: %s.\n", pcap_geterr (pcap)); } @@ -694,11 +701,11 @@ pcl::HDLGrabber::readPacketsFromPcap () } struct timeval lasttime; - unsigned long long usec_delay; + uint64_t usec_delay; lasttime.tv_sec = 0; - int returnValue = pcap_next_ex(pcap, &header, &data); + int32_t returnValue = pcap_next_ex (pcap, &header, &data); while (returnValue >= 0 && !terminate_read_packet_thread_) { @@ -715,15 +722,15 @@ pcl::HDLGrabber::readPacketsFromPcap () usec_delay = ((header->ts.tv_sec - lasttime.tv_sec) * 1000000) + (header->ts.tv_usec - lasttime.tv_usec); - boost::this_thread::sleep(boost::posix_time::microseconds(usec_delay)); + boost::this_thread::sleep (boost::posix_time::microseconds (usec_delay)); lasttime.tv_sec = header->ts.tv_sec; lasttime.tv_usec = header->ts.tv_usec; // The ETHERNET header is 42 bytes long; unnecessary - enqueueHDLPacket(data + 42, header->len - 42); + enqueueHDLPacket (data + 42, header->len - 42); - returnValue = pcap_next_ex(pcap, &header, &data); + returnValue = pcap_next_ex (pcap, &header, &data); } } #endif //#ifdef HAVE_PCAP diff --git a/io/src/vlp_grabber.cpp b/io/src/vlp_grabber.cpp index ec0b64bff19..f996f20d3b7 100644 --- a/io/src/vlp_grabber.cpp +++ b/io/src/vlp_grabber.cpp @@ -40,21 +40,20 @@ using boost::asio::ip::udp; -#define VLP_MAX_NUM_LASERS 16 -#define VLP_DUAL_MODE 0x39 - ///////////////////////////////////////////////////////////////////////////// pcl::VLPGrabber::VLPGrabber (const std::string& pcapFile) : HDLGrabber ("", pcapFile) { + initializeLaserMapping (); loadVLP16Corrections (); } ///////////////////////////////////////////////////////////////////////////// pcl::VLPGrabber::VLPGrabber (const boost::asio::ip::address& ipAddress, - const unsigned short int port) : + const uint16_t port) : HDLGrabber (ipAddress, port) { + initializeLaserMapping (); loadVLP16Corrections (); } @@ -65,12 +64,21 @@ pcl::VLPGrabber::~VLPGrabber () throw () ///////////////////////////////////////////////////////////////////////////// void -pcl::VLPGrabber::loadVLP16Corrections () +pcl::VLPGrabber::initializeLaserMapping () { - double vlp16_vertical_corrections[] = { -15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15 + for (uint8_t i = 0; i < VLP_MAX_NUM_LASERS / 2u; i++) + { + laser_rgb_mapping_[i * 2].b = static_cast (i * 6 + 64); + laser_rgb_mapping_[i * 2 + 1].b = static_cast ((i + 16) * 6 + 64); + } +} - }; - for (int i = 0; i < VLP_MAX_NUM_LASERS; i++) +///////////////////////////////////////////////////////////////////////////// +void +pcl::VLPGrabber::loadVLP16Corrections () +{ + double vlp16_vertical_corrections[] = { -15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15 }; + for (uint8_t i = 0; i < VLP_MAX_NUM_LASERS; i++) { HDLGrabber::laser_corrections_[i].azimuthCorrection = 0.0; HDLGrabber::laser_corrections_[i].distanceCorrection = 0.0; @@ -106,25 +114,25 @@ pcl::VLPGrabber::toPointClouds (HDLDataPacket *dataPacket) double interpolated_azimuth_delta; - int index = 1; + uint8_t index = 1; if (dataPacket->mode == VLP_DUAL_MODE) { index = 2; } if (dataPacket->firingData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition) { - interpolated_azimuth_delta = ( (dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0; + interpolated_azimuth_delta = ((dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0; } else { interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0; } - for (int i = 0; i < HDL_FIRING_PER_PKT; ++i) + for (uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i) { HDLFiringData firing_data = dataPacket->firingData[i]; - for (int j = 0; j < HDL_LASER_PER_FIRING; j++) + for (uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++) { double current_azimuth = firing_data.rotationalPosition; if (j >= VLP_MAX_NUM_LASERS) @@ -139,10 +147,12 @@ pcl::VLPGrabber::toPointClouds (HDLDataPacket *dataPacket) { if (current_sweep_xyz_->size () > 0) { - current_sweep_xyz_->is_dense = current_sweep_xyzi_->is_dense = false; + current_sweep_xyz_->is_dense = current_sweep_xyzrgba_->is_dense = current_sweep_xyzi_->is_dense = false; current_sweep_xyz_->header.stamp = velodyne_time; + current_sweep_xyzrgba_->header.stamp = velodyne_time; current_sweep_xyzi_->header.stamp = velodyne_time; current_sweep_xyz_->header.seq = sweep_counter; + current_sweep_xyzrgba_->header.seq = sweep_counter; current_sweep_xyzi_->header.seq = sweep_counter; sweep_counter++; @@ -150,43 +160,51 @@ pcl::VLPGrabber::toPointClouds (HDLDataPacket *dataPacket) HDLGrabber::fireCurrentSweep (); } current_sweep_xyz_.reset (new pcl::PointCloud ()); + current_sweep_xyzrgba_.reset (new pcl::PointCloud ()); current_sweep_xyzi_.reset (new pcl::PointCloud ()); } PointXYZ xyz; PointXYZI xyzi; + PointXYZRGBA xyzrgba; PointXYZ dual_xyz; PointXYZI dual_xyzi; + PointXYZRGBA dual_xyzrgba; HDLGrabber::computeXYZI (xyzi, current_azimuth, firing_data.laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]); - xyz.x = xyzi.x; - xyz.y = xyzi.y; - xyz.z = xyzi.z; + xyz.x = xyzrgba.x = xyzi.x; + xyz.y = xyzrgba.y = xyzi.y; + xyz.z = xyzrgba.z = xyzi.z; + + xyzrgba.rgba = laser_rgb_mapping_[j % VLP_MAX_NUM_LASERS].rgba; if (dataPacket->mode == VLP_DUAL_MODE) { HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]); - dual_xyz.x = dual_xyzi.x; - dual_xyz.y = dual_xyzi.y; - dual_xyz.z = dual_xyzi.z; + dual_xyz.x = dual_xyzrgba.x = dual_xyzi.x; + dual_xyz.y = dual_xyzrgba.y = dual_xyzi.y; + dual_xyz.z = dual_xyzrgba.z = dual_xyzi.z; + dual_xyzrgba.rgba = laser_rgb_mapping_[j % VLP_MAX_NUM_LASERS].rgba; } if (! (pcl_isnan (xyz.x) || pcl_isnan (xyz.y) || pcl_isnan (xyz.z))) { current_sweep_xyz_->push_back (xyz); + current_sweep_xyzrgba_->push_back (xyzrgba); current_sweep_xyzi_->push_back (xyzi); last_azimuth_ = current_azimuth; } if (dataPacket->mode == VLP_DUAL_MODE) { - if ( (dual_xyz.x != xyz.x || dual_xyz.y != xyz.y || dual_xyz.z != xyz.z) + if ((dual_xyz.x != xyz.x || dual_xyz.y != xyz.y || dual_xyz.z != xyz.z) && ! (pcl_isnan (dual_xyz.x) || pcl_isnan (dual_xyz.y) || pcl_isnan (dual_xyz.z))) { current_sweep_xyz_->push_back (dual_xyz); + current_sweep_xyzrgba_->push_back (dual_xyzrgba); current_sweep_xyzi_->push_back (dual_xyzi); } } @@ -205,3 +223,20 @@ pcl::VLPGrabber::getName () const return (std::string ("Velodyne LiDAR (VLP) Grabber")); } +///////////////////////////////////////////////////////////////////////////// +void +pcl::VLPGrabber::setLaserColorRGB (const pcl::RGB& color, + const uint8_t laserNumber) +{ + if (laserNumber >= VLP_MAX_NUM_LASERS) + return; + + laser_rgb_mapping_[laserNumber] = color; +} + +///////////////////////////////////////////////////////////////////////////// +uint8_t +pcl::VLPGrabber::getMaximumNumberOfLasers () const +{ + return (VLP_MAX_NUM_LASERS); +}