diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt index ca3756ff996..48819b7887f 100644 --- a/io/CMakeLists.txt +++ b/io/CMakeLists.txt @@ -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 @@ -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" diff --git a/io/include/pcl/io/hdl_grabber.h b/io/include/pcl/io/hdl_grabber.h index d86eda29b2c..bdd0c0f775c 100644 --- a/io/include/pcl/io/hdl_grabber.h +++ b/io/include/pcl/io/hdl_grabber.h @@ -62,46 +62,49 @@ 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 >&, - float, float); + typedef void + (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); + typedef void + (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (const boost::shared_ptr >&, + 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 >&, - float startAngle, float); + typedef void + (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 >&); + 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 >&); + 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 >&); + typedef void + (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (const boost::shared_ptr >&); /** \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 @@ -109,61 +112,75 @@ 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 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; @@ -171,8 +188,6 @@ namespace pcl 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 @@ -197,8 +212,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 @@ -214,6 +229,29 @@ 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 > 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::signals2::signal* sweep_xyz_signal_; + boost::signals2::signal* sweep_xyzrgb_signal_; + boost::signals2::signal* sweep_xyzi_signal_; + boost::signals2::signal* scan_xyz_signal_; + boost::signals2::signal* scan_xyzrgb_signal_; + boost::signals2::signal* scan_xyzi_signal_; + private: static double *cos_lookup_table_; static double *sin_lookup_table_; @@ -221,47 +259,38 @@ namespace pcl 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 > current_scan_xyz_, - current_sweep_xyz_; - boost::shared_ptr > current_scan_xyzi_, - current_sweep_xyzi_; - boost::shared_ptr > current_scan_xyzrgb_, - current_sweep_xyzrgb_; - unsigned int last_azimuth_; - boost::signals2::signal* sweep_xyz_signal_; - boost::signals2::signal* sweep_xyzrgb_signal_; - boost::signals2::signal* sweep_xyzi_signal_; - boost::signals2::signal* scan_xyz_signal_; - boost::signals2::signal* scan_xyzrgb_signal_; - boost::signals2::signal* 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(); #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); }; } diff --git a/io/include/pcl/io/vlp_grabber.h b/io/include/pcl/io/vlp_grabber.h new file mode 100644 index 00000000000..ad1bbd521aa --- /dev/null +++ b/io/include/pcl/io/vlp_grabber.h @@ -0,0 +1,92 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012 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. + * + */ + +#include "pcl/pcl_config.h" + +#ifndef PCL_IO_VLP_GRABBER_H_ +#define PCL_IO_VLP_GRABBER_H_ + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + + /** \brief Grabber for the Velodyne LiDAR (VLP), based on the Velodyne High Definition Laser (HDL) + * \author Keven Ring + * \ingroup io + */ + class PCL_EXPORTS VLPGrabber : public HDLGrabber + { + public: + /** \brief Constructor taking an optional path to an vlp corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368] + * \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional + */ + VLPGrabber (const std::string& pcapFile = ""); + + /** \brief Constructor taking a specified IP/port + * \param[in] ipAddress IP Address that should be used to listen for VLP packets + * \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); + + /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ + virtual + ~VLPGrabber () throw (); + + /** \brief Obtains the name of this I/O Grabber + * \return The name of the grabber + */ + virtual std::string + getName () const; + + private: + virtual void + toPointClouds (HDLDataPacket *dataPacket); + boost::asio::ip::address + getDefaultNetworkAddress (); + void + loadVLP16Corrections (); + }; +} + +#endif /* PCL_IO_VLP_GRABBER_H_ */ diff --git a/io/src/hdl_grabber.cpp b/io/src/hdl_grabber.cpp index dbf8a7ef0b9..3a5da360fc4 100644 --- a/io/src/hdl_grabber.cpp +++ b/io/src/hdl_grabber.cpp @@ -49,7 +49,6 @@ #include #endif // #ifdef HAVE_PCAP -const boost::asio::ip::address pcl::HDLGrabber::HDL_DEFAULT_NETWORK_ADDRESS = boost::asio::ip::address::from_string ("192.168.3.255"); double *pcl::HDLGrabber::cos_lookup_table_ = NULL; double *pcl::HDLGrabber::sin_lookup_table_ = NULL; @@ -57,63 +56,63 @@ using boost::asio::ip::udp; ///////////////////////////////////////////////////////////////////////////// pcl::HDLGrabber::HDLGrabber (const std::string& correctionsFile, - const std::string& pcapFile) - : hdl_data_ () - , udp_listener_endpoint_ (HDL_DEFAULT_NETWORK_ADDRESS, HDL_DATA_PORT) - , source_address_filter_ () - , source_port_filter_ (443) - , hdl_read_socket_service_ () - , hdl_read_socket_ (NULL) - , pcap_file_name_ (pcapFile) - , queue_consumer_thread_ (NULL) - , hdl_read_packet_thread_ (NULL) - , 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 ()) - , last_azimuth_ (65000) - , sweep_xyz_signal_ () - , sweep_xyzrgb_signal_ () - , sweep_xyzi_signal_ () - , scan_xyz_signal_ () - , scan_xyzrgb_signal_ () - , scan_xyzi_signal_ () - , min_distance_threshold_(0.0) - , max_distance_threshold_(10000.0) + const std::string& pcapFile) : + 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 ()), + sweep_xyz_signal_ (), + sweep_xyzrgb_signal_ (), + sweep_xyzi_signal_ (), + scan_xyz_signal_ (), + scan_xyzrgb_signal_ (), + scan_xyzi_signal_ (), + hdl_data_ (), + udp_listener_endpoint_ (), + source_address_filter_ (), + source_port_filter_ (443), + hdl_read_socket_service_ (), + hdl_read_socket_ (NULL), + pcap_file_name_ (pcapFile), + queue_consumer_thread_ (NULL), + hdl_read_packet_thread_ (NULL), + min_distance_threshold_ (0.0), + max_distance_threshold_ (10000.0) { initialize (correctionsFile); } ///////////////////////////////////////////////////////////////////////////// pcl::HDLGrabber::HDLGrabber (const boost::asio::ip::address& ipAddress, - const unsigned short int port, - const std::string& correctionsFile) - : hdl_data_ () - , udp_listener_endpoint_ (ipAddress, port) - , source_address_filter_ () - , source_port_filter_ (443) - , hdl_read_socket_service_ () - , hdl_read_socket_ (NULL) - , pcap_file_name_ () - , queue_consumer_thread_ (NULL) - , hdl_read_packet_thread_ (NULL) - , 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 ()) - , last_azimuth_ (65000) - , sweep_xyz_signal_ () - , sweep_xyzrgb_signal_ () - , sweep_xyzi_signal_ () - , scan_xyz_signal_ () - , scan_xyzrgb_signal_ () - , scan_xyzi_signal_ () - , min_distance_threshold_(0.0) - , max_distance_threshold_(10000.0) + const unsigned short int 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 ()), + sweep_xyz_signal_ (), + sweep_xyzrgb_signal_ (), + sweep_xyzi_signal_ (), + scan_xyz_signal_ (), + scan_xyzrgb_signal_ (), + scan_xyzi_signal_ (), + hdl_data_ (), + udp_listener_endpoint_ (ipAddress, port), + source_address_filter_ (), + source_port_filter_ (443), + hdl_read_socket_service_ (), + hdl_read_socket_ (NULL), + pcap_file_name_ (), + queue_consumer_thread_ (NULL), + hdl_read_packet_thread_ (NULL), + min_distance_threshold_ (0.0), + max_distance_threshold_ (10000.0) { initialize (correctionsFile); } @@ -152,14 +151,12 @@ pcl::HDLGrabber::initialize (const std::string& correctionsFile) for (int 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; + 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_xyzi_signal_ =createSignal (); + sweep_xyzi_signal_ = createSignal (); scan_xyz_signal_ = createSignal (); scan_xyzrgb_signal_ = createSignal (); scan_xyzi_signal_ = createSignal (); @@ -225,33 +222,32 @@ pcl::HDLGrabber::loadCorrectionsFile (const std::string& correctionsFile) { if (px.first == "px") { - boost::property_tree::ptree calibrationData = px.second; + boost::property_tree::ptree calibration_data = px.second; int index = -1; - double azimuth = 0, vertCorrection = 0, distCorrection = 0, - vertOffsetCorrection = 0, horizOffsetCorrection = 0; + 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, calibrationData) + BOOST_FOREACH (boost::property_tree::ptree::value_type &item, calibration_data) { if (item.first == "id_") index = atoi (item.second.data ().c_str ()); if (item.first == "rotCorrection_") azimuth = atof (item.second.data ().c_str ()); if (item.first == "vertCorrection_") - vertCorrection = atof (item.second.data ().c_str ()); + vert_correction = atof (item.second.data ().c_str ()); if (item.first == "distCorrection_") - distCorrection = atof (item.second.data ().c_str ()); + dist_correction = atof (item.second.data ().c_str ()); if (item.first == "vertOffsetCorrection_") - vertOffsetCorrection = atof (item.second.data ().c_str ()); + vert_offset_correction = atof (item.second.data ().c_str ()); if (item.first == "horizOffsetCorrection_") - horizOffsetCorrection = atof (item.second.data ().c_str ()); + horiz_offset_correction = atof (item.second.data ().c_str ()); } if (index != -1) { laser_corrections_[index].azimuthCorrection = azimuth; - laser_corrections_[index].verticalCorrection = vertCorrection; - laser_corrections_[index].distanceCorrection = distCorrection / 100.0; - laser_corrections_[index].verticalOffsetCorrection = vertOffsetCorrection / 100.0; - laser_corrections_[index].horizontalOffsetCorrection = horizOffsetCorrection / 100.0; + laser_corrections_[index].verticalCorrection = vert_correction; + laser_corrections_[index].distanceCorrection = dist_correction / 100.0; + laser_corrections_[index].verticalOffsetCorrection = vert_offset_correction / 100.0; + laser_corrections_[index].horizontalOffsetCorrection = horiz_offset_correction / 100.0; laser_corrections_[index].cosVertCorrection = std::cos (HDL_Grabber_toRadians(laser_corrections_[index].verticalCorrection)); laser_corrections_[index].sinVertCorrection = std::sin (HDL_Grabber_toRadians(laser_corrections_[index].verticalCorrection)); @@ -266,20 +262,17 @@ pcl::HDLGrabber::loadCorrectionsFile (const std::string& correctionsFile) void pcl::HDLGrabber::loadHDL32Corrections () { - double hdl32VerticalCorrections[] = { - -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 }; + 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++) { laser_corrections_[i].azimuthCorrection = 0.0; laser_corrections_[i].distanceCorrection = 0.0; laser_corrections_[i].horizontalOffsetCorrection = 0.0; laser_corrections_[i].verticalOffsetCorrection = 0.0; - laser_corrections_[i].verticalCorrection = hdl32VerticalCorrections[i]; - laser_corrections_[i].sinVertCorrection = std::sin (HDL_Grabber_toRadians(hdl32VerticalCorrections[i])); - laser_corrections_[i].cosVertCorrection = std::cos (HDL_Grabber_toRadians(hdl32VerticalCorrections[i])); + laser_corrections_[i].verticalCorrection = hdl32_vertical_corrections[i]; + 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++) { @@ -293,6 +286,13 @@ pcl::HDLGrabber::loadHDL32Corrections () } } +///////////////////////////////////////////////////////////////////////////// +boost::asio::ip::address +pcl::HDLGrabber::getDefaultNetworkAddress () +{ + return (boost::asio::ip::address::from_string ("192.168.3.255")); +} + ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::processVelodynePackets () @@ -313,47 +313,47 @@ pcl::HDLGrabber::processVelodynePackets () void pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket) { - static uint32_t scanCounter = 0; - static uint32_t sweepCounter = 0; - if (sizeof (HDLLaserReturn) != 3) + static uint32_t scan_counter = 0; + static uint32_t sweep_counter = 0; + if (sizeof(HDLLaserReturn) != 3) return; current_scan_xyz_.reset (new pcl::PointCloud ()); current_scan_xyzrgb_.reset (new pcl::PointCloud ()); current_scan_xyzi_.reset (new pcl::PointCloud ()); - time_t time_; - time(&time_); - time_t velodyneTime = (time_ & 0x00000000ffffffffl) << 32 | dataPacket->gpsTimestamp; + time_t system_time; + time (&system_time); + time_t velodyne_time = (system_time & 0x00000000ffffffffl) << 32 | dataPacket->gpsTimestamp; - current_scan_xyz_->header.stamp = velodyneTime; - current_scan_xyzrgb_->header.stamp = velodyneTime; - current_scan_xyzi_->header.stamp = velodyneTime; - current_scan_xyz_->header.seq = scanCounter; - current_scan_xyzrgb_->header.seq = scanCounter; - current_scan_xyzi_->header.seq = scanCounter; - scanCounter++; + current_scan_xyz_->header.stamp = velodyne_time; + current_scan_xyzrgb_->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_xyzi_->header.seq = scan_counter; + scan_counter++; for (int i = 0; i < HDL_FIRING_PER_PKT; ++i) { - HDLFiringData firingData = dataPacket->firingData[i]; - int offset = (firingData.blockIdentifier == BLOCK_0_TO_31) ? 0 : 32; + HDLFiringData firing_data = dataPacket->firingData[i]; + int offset = (firing_data.blockIdentifier == BLOCK_0_TO_31) ? 0 : 32; for (int j = 0; j < HDL_LASER_PER_FIRING; j++) { - if (firingData.rotationalPosition < last_azimuth_) + if (firing_data.rotationalPosition < last_azimuth_) { if (current_sweep_xyzrgb_->size () > 0) { current_sweep_xyz_->is_dense = current_sweep_xyzrgb_->is_dense = current_sweep_xyzi_->is_dense = false; - current_sweep_xyz_->header.stamp = velodyneTime; - current_sweep_xyzrgb_->header.stamp = velodyneTime; - current_sweep_xyzi_->header.stamp = velodyneTime; - current_sweep_xyz_->header.seq = sweepCounter; - current_sweep_xyzrgb_->header.seq = sweepCounter; - current_sweep_xyzi_->header.seq = sweepCounter; + current_sweep_xyz_->header.stamp = velodyne_time; + current_sweep_xyzrgb_->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_xyzi_->header.seq = sweep_counter; - sweepCounter++; + sweep_counter++; fireCurrentSweep (); } @@ -366,16 +366,15 @@ pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket) PointXYZI xyzi; PointXYZRGBA xyzrgb; - computeXYZI (xyzi, firingData.rotationalPosition, firingData.laserReturns[j], laser_corrections_[j + offset]); + 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; xyzrgb.rgba = laser_rgb_mapping_[j + offset].rgba; - if ((boost::math::isnan)(xyz.x) || - (boost::math::isnan)(xyz.y) || - (boost::math::isnan)(xyz.z)) { + if ( (boost::math::isnan) (xyz.x) || (boost::math::isnan) (xyz.y) || (boost::math::isnan) (xyz.z)) + { continue; } @@ -387,70 +386,75 @@ pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket) current_sweep_xyzi_->push_back (xyzi); current_sweep_xyzrgb_->push_back (xyzrgb); - last_azimuth_ = firingData.rotationalPosition; + last_azimuth_ = firing_data.rotationalPosition; } } current_scan_xyz_->is_dense = current_scan_xyzrgb_->is_dense = current_scan_xyzi_->is_dense = true; - fireCurrentScan (dataPacket->firingData[0].rotationalPosition, - dataPacket->firingData[11].rotationalPosition); + fireCurrentScan (dataPacket->firingData[0].rotationalPosition, dataPacket->firingData[11].rotationalPosition); } ///////////////////////////////////////////////////////////////////////////// void -pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point, int azimuth, - HDLLaserReturn laserReturn, HDLLaserCorrection correction) +pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point, + int azimuth, + HDLLaserReturn laserReturn, + HDLLaserCorrection correction) { - double cosAzimuth, sinAzimuth; + double cos_azimuth, sin_azimuth; double distanceM = laserReturn.distance * 0.002; - if (distanceM < min_distance_threshold_ || distanceM > max_distance_threshold_) { - point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); - point.intensity = static_cast (laserReturn.intensity); + point.intensity = static_cast (laserReturn.intensity); + if (distanceM < min_distance_threshold_ || distanceM > max_distance_threshold_) + { + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); return; } if (correction.azimuthCorrection == 0) { - cosAzimuth = cos_lookup_table_[azimuth]; - sinAzimuth = sin_lookup_table_[azimuth]; + cos_azimuth = cos_lookup_table_[azimuth]; + sin_azimuth = sin_lookup_table_[azimuth]; } else { - double azimuthInRadians = HDL_Grabber_toRadians ((static_cast (azimuth) / 100.0) - correction.azimuthCorrection); - cosAzimuth = std::cos (azimuthInRadians); - sinAzimuth = std::sin (azimuthInRadians); + double azimuthInRadians = HDL_Grabber_toRadians( (static_cast (azimuth) / 100.0) - correction.azimuthCorrection); + cos_azimuth = std::cos (azimuthInRadians); + sin_azimuth = std::sin (azimuthInRadians); } distanceM += correction.distanceCorrection; double xyDistance = distanceM * correction.cosVertCorrection; - point.x = static_cast (xyDistance * sinAzimuth - correction.horizontalOffsetCorrection * cosAzimuth); - point.y = static_cast (xyDistance * cosAzimuth + correction.horizontalOffsetCorrection * sinAzimuth); + point.x = static_cast (xyDistance * sin_azimuth - correction.horizontalOffsetCorrection * cos_azimuth); + point.y = static_cast (xyDistance * cos_azimuth + correction.horizontalOffsetCorrection * sin_azimuth); point.z = static_cast (distanceM * correction.sinVertCorrection + correction.verticalOffsetCorrection); - point.intensity = static_cast (laserReturn.intensity); + if (point.x == 0 && point.y == 0 && point.z == 0) + { + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + } } ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::fireCurrentSweep () { - if (sweep_xyz_signal_->num_slots () > 0) + if (sweep_xyz_signal_ != NULL && sweep_xyz_signal_->num_slots () > 0) sweep_xyz_signal_->operator() (current_sweep_xyz_); - - if (sweep_xyzrgb_signal_->num_slots () > 0) + + if (sweep_xyzrgb_signal_ != NULL && sweep_xyzrgb_signal_->num_slots () > 0) sweep_xyzrgb_signal_->operator() (current_sweep_xyzrgb_); - if (sweep_xyzi_signal_->num_slots () > 0) + if (sweep_xyzi_signal_ != NULL && sweep_xyzi_signal_->num_slots () > 0) sweep_xyzi_signal_->operator() (current_sweep_xyzi_); } ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::fireCurrentScan (const unsigned short startAngle, - const unsigned short endAngle) + const unsigned short endAngle) { const float start = static_cast (startAngle) / 100.0f; const float end = static_cast (endAngle) / 100.0f; @@ -468,7 +472,7 @@ pcl::HDLGrabber::fireCurrentScan (const unsigned short startAngle, ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::enqueueHDLPacket (const unsigned char *data, - std::size_t bytesReceived) + std::size_t bytesReceived) { if (bytesReceived == 1206) { @@ -494,19 +498,29 @@ pcl::HDLGrabber::start () { try { - try { - hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp_listener_endpoint_); - } - catch (const std::exception& bind) { - delete hdl_read_socket_; - hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp::endpoint(boost::asio::ip::address_v4::any(), udp_listener_endpoint_.port())); - } + try + { + if (isAddressUnspecified (udp_listener_endpoint_.address ())) + { + udp_listener_endpoint_.address (getDefaultNetworkAddress ()); + } + if (udp_listener_endpoint_.port () == 0) + { + udp_listener_endpoint_.port (HDL_DATA_PORT); + } + hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp_listener_endpoint_); + } + catch (const std::exception& bind) + { + delete hdl_read_socket_; + hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp::endpoint (boost::asio::ip::address_v4::any (), udp_listener_endpoint_.port ())); + } hdl_read_socket_service_.run (); } catch (std::exception &e) { - PCL_ERROR ("[pcl::HDLGrabber::start] Unable to bind to socket! %s\n", e.what()); - return; + PCL_ERROR("[pcl::HDLGrabber::start] Unable to bind to socket! %s\n", e.what ()); + return; } hdl_read_packet_thread_ = new boost::thread (boost::bind (&HDLGrabber::readPacketsFromSocket, this)); } @@ -550,8 +564,7 @@ pcl::HDLGrabber::stop () bool pcl::HDLGrabber::isRunning () const { - return (!hdl_data_.isEmpty() || (hdl_read_packet_thread_ != NULL && - !hdl_read_packet_thread_->timed_join (boost::posix_time::milliseconds (10)))); + return (!hdl_data_.isEmpty () || (hdl_read_packet_thread_ != NULL && !hdl_read_packet_thread_->timed_join (boost::posix_time::milliseconds (10)))); } ///////////////////////////////////////////////////////////////////////////// @@ -596,7 +609,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 @@ -604,26 +617,30 @@ pcl::HDLGrabber::isAddressUnspecified (const boost::asio::ip::address& ipAddress ///////////////////////////////////////////////////////////////////////////// void -pcl::HDLGrabber::setMaximumDistanceThreshold(float &maxThreshold) { +pcl::HDLGrabber::setMaximumDistanceThreshold (float &maxThreshold) +{ max_distance_threshold_ = maxThreshold; } ///////////////////////////////////////////////////////////////////////////// void -pcl::HDLGrabber::setMinimumDistanceThreshold(float &minThreshold) { +pcl::HDLGrabber::setMinimumDistanceThreshold (float &minThreshold) +{ min_distance_threshold_ = minThreshold; } ///////////////////////////////////////////////////////////////////////////// float -pcl::HDLGrabber::getMaximumDistanceThreshold() { - return(max_distance_threshold_); +pcl::HDLGrabber::getMaximumDistanceThreshold () +{ + return (max_distance_threshold_); } ///////////////////////////////////////////////////////////////////////////// float -pcl::HDLGrabber::getMinimumDistanceThreshold() { - return(min_distance_threshold_); +pcl::HDLGrabber::getMinimumDistanceThreshold () +{ + return (min_distance_threshold_); } ///////////////////////////////////////////////////////////////////////////// @@ -633,15 +650,15 @@ pcl::HDLGrabber::readPacketsFromSocket () unsigned char data[1500]; udp::endpoint sender_endpoint; - while (!terminate_read_packet_thread_ && hdl_read_socket_->is_open()) + while (!terminate_read_packet_thread_ && hdl_read_socket_->is_open ()) { size_t length = hdl_read_socket_->receive_from (boost::asio::buffer (data, 1500), sender_endpoint); - if (isAddressUnspecified (source_address_filter_) || - (source_address_filter_ == sender_endpoint.address () && source_port_filter_ == sender_endpoint.port ())) - { + if (isAddressUnspecified (source_address_filter_) + || (source_address_filter_ == sender_endpoint.address () && source_port_filter_ == sender_endpoint.port ())) + { enqueueHDLPacket (data, length); - } + } } } @@ -657,16 +674,16 @@ pcl::HDLGrabber::readPacketsFromPcap () pcap_t *pcap = pcap_open_offline (pcap_file_name_.c_str (), errbuff); struct bpf_program filter; - std::ostringstream stringStream; + std::ostringstream string_stream; - stringStream << "udp "; + string_stream << "udp "; if (!isAddressUnspecified(source_address_filter_)) { - stringStream << " 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, stringStream.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)); } @@ -676,7 +693,7 @@ pcl::HDLGrabber::readPacketsFromPcap () } struct timeval lasttime; - unsigned long long uSecDelay; + unsigned long long usec_delay; lasttime.tv_sec = 0; @@ -694,10 +711,10 @@ pcl::HDLGrabber::readPacketsFromPcap () lasttime.tv_usec -= 1000000; lasttime.tv_sec++; } - uSecDelay = ((header->ts.tv_sec - lasttime.tv_sec) * 1000000) + - (header->ts.tv_usec - lasttime.tv_usec); + 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(uSecDelay)); + boost::this_thread::sleep(boost::posix_time::microseconds(usec_delay)); lasttime.tv_sec = header->ts.tv_sec; lasttime.tv_usec = header->ts.tv_usec; diff --git a/io/src/vlp_grabber.cpp b/io/src/vlp_grabber.cpp new file mode 100644 index 00000000000..d32141b73fb --- /dev/null +++ b/io/src/vlp_grabber.cpp @@ -0,0 +1,207 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015-, Open Perception, Inc. + * Copyright (c) 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. + * + */ + +#include + +using boost::asio::ip::udp; + +#define VLP_MAX_NUM_LASERS 16 +#define VLP_DUAL_MODE 0x39 + +///////////////////////////////////////////////////////////////////////////// +pcl::VLPGrabber::VLPGrabber (const std::string& pcapFile) : + pcl::HDLGrabber::HDLGrabber ("", pcapFile) +{ + loadVLP16Corrections (); +} + +///////////////////////////////////////////////////////////////////////////// +pcl::VLPGrabber::VLPGrabber (const boost::asio::ip::address& ipAddress, + const unsigned short int port) : + pcl::HDLGrabber::HDLGrabber (ipAddress, port) +{ + loadVLP16Corrections (); +} + +///////////////////////////////////////////////////////////////////////////// +pcl::VLPGrabber::~VLPGrabber () throw () +{ +} + +///////////////////////////////////////////////////////////////////////////// +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 (int i = 0; i < VLP_MAX_NUM_LASERS; i++) + { + HDLGrabber::laser_corrections_[i].azimuthCorrection = 0.0; + HDLGrabber::laser_corrections_[i].distanceCorrection = 0.0; + HDLGrabber::laser_corrections_[i].horizontalOffsetCorrection = 0.0; + HDLGrabber::laser_corrections_[i].verticalOffsetCorrection = 0.0; + HDLGrabber::laser_corrections_[i].verticalCorrection = vlp16_vertical_corrections[i]; + HDLGrabber::laser_corrections_[i].sinVertCorrection = std::sin (HDL_Grabber_toRadians(vlp16_vertical_corrections[i])); + HDLGrabber::laser_corrections_[i].cosVertCorrection = std::cos (HDL_Grabber_toRadians(vlp16_vertical_corrections[i])); + } +} + +///////////////////////////////////////////////////////////////////////////// +boost::asio::ip::address +pcl::VLPGrabber::getDefaultNetworkAddress () +{ + return (boost::asio::ip::address::from_string ("255.255.255.255")); +} + +///////////////////////////////////////////////////////////////////////////// +void +pcl::VLPGrabber::toPointClouds (HDLDataPacket *dataPacket) +{ + static uint32_t scan_counter = 0; + static uint32_t sweep_counter = 0; + if (sizeof(HDLLaserReturn) != 3) + return; + + time_t system_time; + time (&system_time); + time_t velodyne_time = (system_time & 0x00000000ffffffffl) << 32 | dataPacket->gpsTimestamp; + + scan_counter++; + + double interpolated_azimuth_delta; + + int 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; + } + else + { + interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0; + } + + for (int i = 0; i < HDL_FIRING_PER_PKT; ++i) + { + HDLFiringData firing_data = dataPacket->firingData[i]; + + for (int j = 0; j < HDL_LASER_PER_FIRING; j++) + { + double current_azimuth = firing_data.rotationalPosition; + if (j > VLP_MAX_NUM_LASERS) + { + current_azimuth += interpolated_azimuth_delta; + } + if (current_azimuth > 36000) + { + current_azimuth -= 36000; + } + if (current_azimuth < HDLGrabber::last_azimuth_) + { + if (current_sweep_xyz_->size () > 0) + { + current_sweep_xyz_->is_dense = current_sweep_xyzi_->is_dense = false; + current_sweep_xyz_->header.stamp = velodyne_time; + current_sweep_xyzi_->header.stamp = velodyne_time; + current_sweep_xyz_->header.seq = sweep_counter; + current_sweep_xyzi_->header.seq = sweep_counter; + + sweep_counter++; + + HDLGrabber::fireCurrentSweep (); + } + current_sweep_xyz_.reset (new pcl::PointCloud ()); + current_sweep_xyzi_.reset (new pcl::PointCloud ()); + } + + PointXYZ xyz; + PointXYZI xyzi; + PointXYZ dual_xyz; + PointXYZI dual_xyzi; + + 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; + + 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; + + } + + if (! (boost::math::isnan (xyz.x) || boost::math::isnan (xyz.y) || boost::math::isnan (xyz.z))) + { + current_sweep_xyz_->push_back (xyz); + 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) + && ! (boost::math::isnan (dual_xyz.x) || boost::math::isnan (dual_xyz.y) || boost::math::isnan (dual_xyz.z))) + { + current_sweep_xyz_->push_back (dual_xyz); + current_sweep_xyzi_->push_back (dual_xyzi); + } + } + } + if (dataPacket->mode == VLP_DUAL_MODE) + { + i++; + } + } +} + +///////////////////////////////////////////////////////////////////////////// +std::string +pcl::VLPGrabber::getName () const +{ + return (std::string ("Velodyne LiDAR (VLP) Grabber")); +} + diff --git a/visualization/tools/CMakeLists.txt b/visualization/tools/CMakeLists.txt index 319217347e1..2b515f58d49 100644 --- a/visualization/tools/CMakeLists.txt +++ b/visualization/tools/CMakeLists.txt @@ -10,6 +10,9 @@ target_link_libraries(pcl_timed_trigger_test pcl_io pcl_common pcl_kdtree pcl_vi PCL_ADD_EXECUTABLE(pcl_hdl_viewer_simple ${SUBSYS_NAME} hdl_viewer_simple.cpp) target_link_libraries(pcl_hdl_viewer_simple pcl_io pcl_common pcl_visualization) +PCL_ADD_EXECUTABLE(pcl_vlp_viewer ${SUBSYS_NAME} vlp_viewer.cpp) +target_link_libraries(pcl_vlp_viewer pcl_io pcl_common pcl_visualization) + if(WITH_OPENNI) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_pcd_grabber_viewer ${SUBSYS_NAME} pcd_grabber_viewer.cpp) diff --git a/visualization/tools/vlp_viewer.cpp b/visualization/tools/vlp_viewer.cpp new file mode 100644 index 00000000000..9af3a68e835 --- /dev/null +++ b/visualization/tools/vlp_viewer.cpp @@ -0,0 +1,237 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015-, Open Perception, Inc. + * Copyright (c) 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 Willow Garage, Inc. 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. + * + * Author: Keven Ring + */ + +#include +#include +#include //fps calculations +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace pcl; +using namespace pcl::console; +using namespace pcl::visualization; + +#define SHOW_FPS 0 +#if SHOW_FPS +#define FPS_CALC(_WHAT_) \ +do \ +{ \ + static unsigned count = 0;\ + static double last = getTime ();\ + double now = getTime (); \ + ++count; \ + if (now - last >= 1.0) \ + { \ + std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \ + count = 0; \ + last = now; \ + } \ +}while(false) +#else +#define FPS_CALC(_WHAT_) \ +do \ +{ \ +}while(false) +#endif + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class SimpleVLPViewer +{ + public: + typedef PointCloud Cloud; + typedef typename Cloud::ConstPtr CloudConstPtr; + typedef typename Cloud::Ptr CloudPtr; + + SimpleVLPViewer (Grabber& grabber, + PointCloudColorHandler *handler) : + cloud_viewer_ (new PCLVisualizer ("PCL HDL Cloud")), + grabber_ (grabber), + handler_ (handler) + { + } + + void + cloud_callback (const CloudConstPtr& cloud) + { + FPS_CALC("cloud callback"); + boost::mutex::scoped_lock lock (cloud_mutex_); + cloud_ = cloud; + } + + void + keyboard_callback (const KeyboardEvent& event, + void* /*cookie*/) + { + if (event.keyUp ()) + { + switch (event.getKeyCode ()) + { + case '0': + delete handler_; + handler_ = new PointCloudColorHandlerCustom (255, 255, 255); + break; + case '1': + delete handler_; + handler_ = new PointCloudColorHandlerGenericField ("x"); + break; + case '2': + delete handler_; + handler_ = new PointCloudColorHandlerGenericField ("y"); + break; + case '3': + delete handler_; + handler_ = new PointCloudColorHandlerGenericField ("z"); + break; + case '4': + delete handler_; + handler_ = new PointCloudColorHandlerGenericField ("intensity"); + break; + case 'a': + cloud_viewer_->removeAllCoordinateSystems (); + cloud_viewer_->addCoordinateSystem (1.0, "global"); + break; + case 'A': + cloud_viewer_->removeAllCoordinateSystems (); + break; + } + } + } + + void + run () + { + cloud_viewer_->addCoordinateSystem (1.0, "global"); + cloud_viewer_->setBackgroundColor (0, 0, 0); + cloud_viewer_->initCameraParameters (); + cloud_viewer_->setCameraPosition (0.0, 0.0, 30.0, 0.0, 1.0, 0.0, 0); + cloud_viewer_->setCameraClipDistances (0.0, 50.0); + cloud_viewer_->registerKeyboardCallback (&SimpleVLPViewer::keyboard_callback, *this); + + boost::function cloud_cb = boost::bind (&SimpleVLPViewer::cloud_callback, this, _1); + boost::signals2::connection cloud_connection = grabber_.registerCallback (cloud_cb); + + grabber_.start (); + + while (!cloud_viewer_->wasStopped ()) + { + CloudConstPtr tmp, cloud; + + if (cloud_mutex_.try_lock ()) + { + cloud_.swap (cloud); + cloud_mutex_.unlock (); + } + + if (cloud) + { + FPS_CALC("drawing cloud"); + handler_->setInputCloud (cloud); + if (!cloud_viewer_->updatePointCloud (cloud, *handler_, "VLP")) + cloud_viewer_->addPointCloud (cloud, *handler_, "VLP"); + + cloud_viewer_->spinOnce (); + } + + if (!grabber_.isRunning ()) + cloud_viewer_->spin (); + + boost::this_thread::sleep (boost::posix_time::microseconds (100)); + } + + grabber_.stop (); + + cloud_connection.disconnect (); + } + + boost::shared_ptr cloud_viewer_; + boost::shared_ptr image_viewer_; + + Grabber& grabber_; + boost::mutex cloud_mutex_; + boost::mutex image_mutex_; + + CloudConstPtr cloud_; + PointCloudColorHandler *handler_; +}; + +void +usage (char ** argv) +{ + cout << "usage: " << argv[0] << " [-pcapFile ] [-h | --help]" << endl; + cout << argv[0] << " -h | --help : shows this help" << endl; + return; +} + +int +main (int argc, + char ** argv) +{ + std::string pcapFile; + + if (find_switch (argc, argv, "-h") || find_switch (argc, argv, "--help")) + { + usage (argv); + return (0); + } + + parse_argument (argc, argv, "-pcapFile", pcapFile); + + VLPGrabber grabber (pcapFile); + + PointCloudColorHandlerGenericField *color_handler = new PointCloudColorHandlerGenericField ("intensity"); + + SimpleVLPViewer v (grabber, color_handler); + v.run (); + + return (0); +} +