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

Added Velodyne VLP Grabber and example viewer #1452

Merged
merged 1 commit into from
Dec 21, 2015
Merged

Added Velodyne VLP Grabber and example viewer #1452

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
2 changes: 2 additions & 0 deletions io/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -212,6 +212,7 @@ if(build)
src/ifs_io.cpp
src/image_grabber.cpp
src/hdl_grabber.cpp
src/vlp_grabber.cpp
src/robot_eye_grabber.cpp
src/file_io.cpp
src/auto_io.cpp
Expand Down Expand Up @@ -260,6 +261,7 @@ if(build)
"include/pcl/${SUBSYS_NAME}/ifs_io.h"
"include/pcl/${SUBSYS_NAME}/image_grabber.h"
"include/pcl/${SUBSYS_NAME}/hdl_grabber.h"
"include/pcl/${SUBSYS_NAME}/vlp_grabber.h"
"include/pcl/${SUBSYS_NAME}/robot_eye_grabber.h"
"include/pcl/${SUBSYS_NAME}/point_cloud_image_extractors.h"
"include/pcl/${SUBSYS_NAME}/io_exception.h"
Expand Down
173 changes: 109 additions & 64 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 @@ -2,7 +2,8 @@
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012 The MITRE Corporation
* Copyright (c) 2012-, Open Perception, Inc.
* Copyright (c) 2012,2015 The MITRE Corporation
*
* All rights reserved.
*
Expand Down Expand Up @@ -62,116 +63,132 @@ namespace pcl
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne
*/
typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyz) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
float, float);
typedef void
(sig_cb_velodyne_hdl_scan_point_cloud_xyz) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&,
float,
float);
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne. Each laser has a different RGB
*/
typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
float, float);
typedef void
(sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&,
float,
float);
/** \brief Signal used for a single sector
* Represents 1 corrected packet from the HDL Velodyne with the returned intensity.
*/
typedef void (sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
float startAngle, float);
typedef void
(sig_cb_velodyne_hdl_scan_point_cloud_xyzi) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&,
float startAngle,
float);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne
* This signal is sent when the Velodyne passes angle "0"
*/
typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
typedef void
(sig_cb_velodyne_hdl_sweep_point_cloud_xyz) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne with the returned intensity
* This signal is sent when the Velodyne passes angle "0"
*/
typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
typedef void
(sig_cb_velodyne_hdl_sweep_point_cloud_xyzi) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
/** \brief Signal used for a 360 degree sweep
* Represents multiple corrected packets from the HDL Velodyne
* This signal is sent when the Velodyne passes angle "0". Each laser has a different RGB
*/
typedef void (sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
typedef void
(sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);

/** \brief Constructor taking an optional path to an HDL corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368]
* \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL. This parameter is mandatory for the HDL-64, optional for the HDL-32
* \param[in] pcapFile Path to a file which contains previously captured data packets. This parameter is optional
*/
HDLGrabber (const std::string& correctionsFile = "",
const std::string& pcapFile = "");
const std::string& pcapFile = "");

/** \brief Constructor taking a pecified IP/port and an optional path to an HDL corrections file.
* \param[in] ipAddress IP Address that should be used to listen for HDL packets
* \param[in] port UDP Port that should be used to listen for HDL packets
* \param[in] correctionsFile Path to a file which contains the correction parameters for the HDL. This field is mandatory for the HDL-64, optional for the HDL-32
*/
HDLGrabber (const boost::asio::ip::address& ipAddress,
const unsigned short port, const std::string& correctionsFile = "");
const unsigned short port,
const std::string& correctionsFile = "");

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

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

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

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

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

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

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

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

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

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

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

float getMinimumDistanceThreshold();
float
getMinimumDistanceThreshold ();

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

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

enum HDLBlock
{
Expand All @@ -197,8 +214,8 @@ namespace pcl
{
HDLFiringData firingData[HDL_FIRING_PER_PKT];
unsigned int gpsTimestamp;
unsigned char blank1;
unsigned char blank2;
unsigned char mode;
unsigned char sensorType;
};

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

HDLLaserCorrection laser_corrections_[HDL_MAX_NUM_LASERS];
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why do you make all these public?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't it protected?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Right, got lost in all the foldings ;).

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

void
fireCurrentSweep ();

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


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

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

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

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

void
processVelodynePackets ();

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

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

void
loadHDL32Corrections ();

void
readPacketsFromSocket ();

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

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

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

};
}

Expand Down
Loading