From 425f5b27f9e00e7de8e11be05b9606546b47971e Mon Sep 17 00:00:00 2001 From: Denis Dillenberger Date: Fri, 10 Oct 2014 15:59:33 +0200 Subject: [PATCH 01/66] parameter and code added for working with multiple velodynes --- .../include/velodyne_driver/input.h | 14 +++++++- velodyne_driver/src/driver/driver.cc | 6 ++++ velodyne_driver/src/lib/input.cc | 32 +++++++++++++++++-- 3 files changed, 48 insertions(+), 4 deletions(-) diff --git a/velodyne_driver/include/velodyne_driver/input.h b/velodyne_driver/include/velodyne_driver/input.h index d62dff7e6..728075fc4 100644 --- a/velodyne_driver/include/velodyne_driver/input.h +++ b/velodyne_driver/include/velodyne_driver/input.h @@ -57,6 +57,15 @@ namespace velodyne_driver * > 0 if incomplete packet (is this possible?) */ virtual int getPacket(velodyne_msgs::VelodynePacket *pkt) = 0; + + + /** @brief Set source IP, from where packets are accepted + * + * @param ip IP of a Velodyne LIDAR e.g. 192.168.51.70 + */ + virtual void setDeviceIP( const std::string& ip ) { devip_str_ = ip; } + protected: + std::string devip_str_; }; /** @brief Live Velodyne input from socket. */ @@ -68,10 +77,11 @@ namespace velodyne_driver ~InputSocket(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); - + void setDeviceIP( const std::string& ip ); private: int sockfd_; + in_addr devip_; }; @@ -92,6 +102,7 @@ namespace velodyne_driver ~InputPCAP(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); + void setDeviceIP( const std::string& ip ); private: @@ -104,6 +115,7 @@ namespace velodyne_driver bool read_fast_; double repeat_delay_; ros::Rate packet_rate_; + bpf_program velodyne_pointdata_filter_; }; } // velodyne_driver namespace diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index 1d04d5d2c..e90defaf7 100644 --- a/velodyne_driver/src/driver/driver.cc +++ b/velodyne_driver/src/driver/driver.cc @@ -95,6 +95,12 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, input_.reset(new velodyne_driver::InputSocket(private_nh)); } + std::string devip; + private_nh.param("device_ip", devip, std::string("")); + if(!devip.empty()) + ROS_INFO_STREAM("Set device ip to " << devip << ", only accepting packets from this address." ); + input_->setDeviceIP(devip); + // raw data output topic output_ = node.advertise("velodyne_packets", 10); } diff --git a/velodyne_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index 8f7d22493..d11bac1e1 100644 --- a/velodyne_driver/src/lib/input.cc +++ b/velodyne_driver/src/lib/input.cc @@ -84,6 +84,12 @@ namespace velodyne_driver (void) close(sockfd_); } + void InputSocket::setDeviceIP(const std::string &ip) + { + devip_str_ = ip; + inet_aton(ip.c_str(),&devip_); + } + /** @brief Get one velodyne packet. */ int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt) { @@ -94,6 +100,9 @@ namespace velodyne_driver fds[0].events = POLLIN; static const int POLL_TIMEOUT = 1000; // one second (in msec) + sockaddr_in sender_address; + socklen_t sender_address_len = sizeof(sender_address); + while (true) { // Unfortunately, the Linux kernel recvfrom() implementation @@ -140,7 +149,8 @@ namespace velodyne_driver // Receive packets that should now be available from the // socket using a blocking read. ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], - packet_size, 0, NULL, NULL); + packet_size, 0, + (sockaddr*) &sender_address, &sender_address_len); if (nbytes < 0) { @@ -153,8 +163,14 @@ namespace velodyne_driver } else if ((size_t) nbytes == packet_size) { - // read successful, done now - break; + // read successful, + // if packet is not from the lidar scanner we selected by IP, + // continue otherwise we are done + std::cout << sender_address.sin_addr.s_addr << " " << devip_.s_addr << std::endl; + if( devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr ) + continue; + else + break; //done } ROS_DEBUG_STREAM("incomplete Velodyne packet read: " @@ -225,6 +241,12 @@ namespace velodyne_driver pcap_close(pcap_); } + void InputPCAP::setDeviceIP(const std::string &ip) + { + std::string filter_str = "src host " + devip_str_ + " && udp src port 2368 && udp dst port 2368"; + if( devip_str_ != "" ) + pcap_compile(pcap_, &velodyne_pointdata_filter_, filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN); + } /** @brief Get one velodyne packet. */ int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt) @@ -237,6 +259,10 @@ namespace velodyne_driver int res; if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) { + // if packet is not from the lidar scanner we selected by IP, continue + if( !devip_str_.empty() && (pcap_offline_filter( &velodyne_pointdata_filter_, header, pkt_data ) == 0) ) + continue; + // Keep the reader from blowing through the file. if (read_fast_ == false) packet_rate_.sleep(); From e1aa46a17d5cc45c0570d057b8b017fb01dc3426 Mon Sep 17 00:00:00 2001 From: Denis Dillenberger Date: Sat, 11 Oct 2014 15:22:10 +0200 Subject: [PATCH 02/66] cleanup debug line --- velodyne_driver/src/lib/input.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/velodyne_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index d11bac1e1..a18714d10 100644 --- a/velodyne_driver/src/lib/input.cc +++ b/velodyne_driver/src/lib/input.cc @@ -166,7 +166,6 @@ namespace velodyne_driver // read successful, // if packet is not from the lidar scanner we selected by IP, // continue otherwise we are done - std::cout << sender_address.sin_addr.s_addr << " " << devip_.s_addr << std::endl; if( devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr ) continue; else From f366bcaca92e2a84cdb650ff3aaf5346f281f52f Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Wed, 7 Jan 2015 16:45:45 -0800 Subject: [PATCH 03/66] fixed missing header --- velodyne_driver/include/velodyne_driver/input.h | 1 + 1 file changed, 1 insertion(+) diff --git a/velodyne_driver/include/velodyne_driver/input.h b/velodyne_driver/include/velodyne_driver/input.h index 728075fc4..eaab2e2a1 100644 --- a/velodyne_driver/include/velodyne_driver/input.h +++ b/velodyne_driver/include/velodyne_driver/input.h @@ -34,6 +34,7 @@ #include #include #include +#include #include #include From d48c203e3bcbca364254bb25a72ab71e30388ba6 Mon Sep 17 00:00:00 2001 From: junior Date: Wed, 7 Jan 2015 20:11:05 -0800 Subject: [PATCH 04/66] parameters to set the udp port --- velodyne_driver/src/driver/driver.cc | 5 ++++- velodyne_driver/src/lib/input.cc | 14 +++++++------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index e90defaf7..6ab39176a 100644 --- a/velodyne_driver/src/driver/driver.cc +++ b/velodyne_driver/src/driver/driver.cc @@ -69,6 +69,9 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, std::string dump_file; private_nh.param("pcap", dump_file, std::string("")); + int udp_port; + private_nh.param("port", udp_port, (int)UDP_PORT_NUMBER); + // initialize diagnostics diagnostics_.setHardwareID(deviceName); const double diag_freq = packet_rate/config_.npackets; @@ -92,7 +95,7 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, } else { - input_.reset(new velodyne_driver::InputSocket(private_nh)); + input_.reset(new velodyne_driver::InputSocket(private_nh, udp_port)); } std::string devip; diff --git a/velodyne_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index a18714d10..383401d2a 100644 --- a/velodyne_driver/src/lib/input.cc +++ b/velodyne_driver/src/lib/input.cc @@ -152,16 +152,16 @@ namespace velodyne_driver packet_size, 0, (sockaddr*) &sender_address, &sender_address_len); - if (nbytes < 0) - { + if (nbytes < 0) + { if (errno != EWOULDBLOCK) - { + { perror("recvfail"); - ROS_INFO("recvfail"); + ROS_INFO("recvfail"); return 1; - } - } - else if ((size_t) nbytes == packet_size) + } + } + else if ((size_t) nbytes == packet_size) { // read successful, // if packet is not from the lidar scanner we selected by IP, From d65ef885ff3f4fbe20d24be9dd31238b009d8642 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Wed, 14 Oct 2015 15:38:25 -0500 Subject: [PATCH 05/66] permit min_range settings below 0.9 meters (#60) No known models are currently known to return closer measurements. --- velodyne_pointcloud/cfg/VelodyneConfig.cfg | 4 ++-- velodyne_pointcloud/launch/VLP16_points.launch | 7 +++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/velodyne_pointcloud/cfg/VelodyneConfig.cfg b/velodyne_pointcloud/cfg/VelodyneConfig.cfg index 5fa57e9b5..57493162b 100755 --- a/velodyne_pointcloud/cfg/VelodyneConfig.cfg +++ b/velodyne_pointcloud/cfg/VelodyneConfig.cfg @@ -6,8 +6,8 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.9, 10.0) -gen.add("max_range", double_t, 0, "max range to publish", 130, 0.9, 200) +gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.1, 10.0) +gen.add("max_range", double_t, 0, "max range to publish", 130, 0.1, 200) gen.add("view_direction", double_t, 0, "angle defining the center of view", 0.0, -pi, pi) gen.add("view_width", double_t, 0, "angle defining the view width", diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index 251fb86b9..e2af36229 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -3,8 +3,6 @@ arg: calibration = path to calibration file (default: standard VLP16db.yaml) pcap = path to packet capture file (default: use real device) - - $Id$ --> @@ -12,7 +10,7 @@ - + @@ -23,8 +21,9 @@ - + + From 85228edd84c1f626c020863b77f1dbb4da3f1ea3 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Wed, 14 Oct 2015 16:21:22 -0500 Subject: [PATCH 06/66] restore VLP-16 min_range setting to 0.4 (#60) NOTE: There is still some other problem keeping that from working. --- velodyne_pointcloud/launch/VLP16_points.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index e2af36229..74b190a6b 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -10,7 +10,7 @@ - + From 70a8fae19e8ed6e9ec8c8caa92e5525d8f308f4c Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Sun, 1 Nov 2015 13:54:08 -0600 Subject: [PATCH 07/66] VLP-16: skip badly formatted data packets (#62, #63) --- .../include/velodyne_pointcloud/rawdata.h | 1 + velodyne_pointcloud/src/lib/rawdata.cc | 12 +++++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index b4fb077b6..3dfbe8f64 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h @@ -55,6 +55,7 @@ namespace velodyne_rawdata static const float DISTANCE_RESOLUTION = 0.002f; /**< meters */ static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0); + /** @todo make this work for both big and little-endian machines */ static const uint16_t UPPER_BANK = 0xeeff; static const uint16_t LOWER_BANK = 0xddff; diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index ec6fc6aaf..c8d96b6a5 100755 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -295,7 +295,17 @@ namespace velodyne_rawdata const raw_packet_t *raw = (const raw_packet_t *) &pkt.data[0]; for (int block = 0; block < BLOCKS_PER_PACKET; block++) { - assert(0xEEFF == raw->blocks[block].header); + + // ignore packets with mangled or otherwise different contents + if (UPPER_BANK != raw->blocks[block].header) { + // Do not flood the log with messages, only issue at most one + // of these warnings per minute. + ROS_WARN_STREAM_THROTTLE(60, "skipping invalid VLP-16 packet: block " + << block << " header value is " + << raw->blocks[block].header); + return; // bad packet: skip the rest + } + azimuth = (float)(raw->blocks[block].rotation); if (block < (BLOCKS_PER_PACKET-1)){ azimuth_diff = (float)((36000 + raw->blocks[block+1].rotation - raw->blocks[block].rotation)%36000); From f3b19fdff1309061b82b735f7d5e81df5b8b17e2 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Fri, 20 Nov 2015 20:30:07 -0600 Subject: [PATCH 08/66] velodyne_driver: use port number for PCAP data (#66) --- .../include/velodyne_driver/input.h | 39 +++---- velodyne_driver/src/driver/driver.cc | 22 ++-- velodyne_driver/src/lib/input.cc | 104 ++++++++++-------- 3 files changed, 83 insertions(+), 82 deletions(-) diff --git a/velodyne_driver/include/velodyne_driver/input.h b/velodyne_driver/include/velodyne_driver/input.h index eaab2e2a1..6d3bc2e80 100644 --- a/velodyne_driver/include/velodyne_driver/input.h +++ b/velodyne_driver/include/velodyne_driver/input.h @@ -2,6 +2,7 @@ * * Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson * Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin + * Copyright (C) 2015, Jack O'Quin * * License: Modified BSD Software License Agreement * @@ -18,7 +19,7 @@ * * Classes: * - * velodyne::Input -- pure virtual base class to access the data + * velodyne::Input -- base class for accessing the data * independently of its source * * velodyne::InputSocket -- derived class reads live data from the @@ -41,13 +42,14 @@ namespace velodyne_driver { - static uint16_t UDP_PORT_NUMBER = 2368; + static uint16_t DATA_PORT_NUMBER = 2368; // default data port + static uint16_t POSITION_PORT_NUMBER = 8308; // default position port - /** @brief Pure virtual Velodyne input base class */ + /** @brief Velodyne input base class */ class Input { public: - Input() {} + Input(ros::NodeHandle private_nh, int port); /** @brief Read one Velodyne packet. * @@ -59,13 +61,9 @@ namespace velodyne_driver */ virtual int getPacket(velodyne_msgs::VelodynePacket *pkt) = 0; - - /** @brief Set source IP, from where packets are accepted - * - * @param ip IP of a Velodyne LIDAR e.g. 192.168.51.70 - */ - virtual void setDeviceIP( const std::string& ip ) { devip_str_ = ip; } protected: + ros::NodeHandle private_nh_; + int port_; std::string devip_str_; }; @@ -73,14 +71,12 @@ namespace velodyne_driver class InputSocket: public Input { public: - InputSocket(ros::NodeHandle private_nh, - uint16_t udp_port = UDP_PORT_NUMBER); + InputSocket(ros::NodeHandle private_nh, int port); ~InputSocket(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); - void setDeviceIP( const std::string& ip ); - private: + private: int sockfd_; in_addr devip_; }; @@ -94,29 +90,22 @@ namespace velodyne_driver class InputPCAP: public Input { public: - InputPCAP(ros::NodeHandle private_nh, - double packet_rate, - std::string filename="", - bool read_once=false, - bool read_fast=false, - double repeat_delay=0.0); + InputPCAP(ros::NodeHandle private_nh, int port, + double packet_rate, std::string filename=""); ~InputPCAP(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); - void setDeviceIP( const std::string& ip ); private: - + ros::Rate packet_rate_; std::string filename_; - FILE *fp_; pcap_t *pcap_; + bpf_program pcap_packet_filter_; char errbuf_[PCAP_ERRBUF_SIZE]; bool empty_; bool read_once_; bool read_fast_; double repeat_delay_; - ros::Rate packet_rate_; - bpf_program velodyne_pointdata_filter_; }; } // velodyne_driver namespace diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index 7bfa8eaa4..e999e8467 100644 --- a/velodyne_driver/src/driver/driver.cc +++ b/velodyne_driver/src/driver/driver.cc @@ -79,7 +79,7 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, private_nh.param("pcap", dump_file, std::string("")); int udp_port; - private_nh.param("port", udp_port, (int)UDP_PORT_NUMBER); + private_nh.param("port", udp_port, (int) DATA_PORT_NUMBER); // initialize diagnostics diagnostics_.setHardwareID(deviceName); @@ -96,25 +96,21 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, TimeStampStatusParam())); // open Velodyne input device or file - if (dump_file != "") + if (dump_file != "") // have PCAP file? { - input_.reset(new velodyne_driver::InputPCAP(private_nh, - packet_rate, - dump_file)); + // read data from packet capture file + input_.reset(new velodyne_driver::InputPCAP(private_nh, udp_port, + packet_rate, dump_file)); } else { + // read data from live socket input_.reset(new velodyne_driver::InputSocket(private_nh, udp_port)); } - std::string devip; - private_nh.param("device_ip", devip, std::string("")); - if(!devip.empty()) - ROS_INFO_STREAM("Set device ip to " << devip << ", only accepting packets from this address." ); - input_->setDeviceIP(devip); - - // raw data output topic - output_ = node.advertise("velodyne_packets", 10); + // raw packet output topic + output_ = + node.advertise("velodyne_packets", 10); } /** poll the device diff --git a/velodyne_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index 383401d2a..c2a8ff5db 100644 --- a/velodyne_driver/src/lib/input.cc +++ b/velodyne_driver/src/lib/input.cc @@ -1,6 +1,7 @@ /* * Copyright (C) 2007 Austin Robot Technology, Patrick Beeson * Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin + * Copyright (C) 2015, Jack O'Quin * * License: Modified BSD Software License Agreement * @@ -11,8 +12,8 @@ * * Input classes for the Velodyne HDL-64E 3D LIDAR: * - * Input -- virtual base class than can be used to access the data - * independently of its source + * Input -- base class used to access the data independently of + * its source * * InputSocket -- derived class reads live data from the device * via a UDP socket @@ -22,6 +23,8 @@ */ #include +#include +#include #include #include #include @@ -32,7 +35,27 @@ namespace velodyne_driver { - static const size_t packet_size = sizeof(velodyne_msgs::VelodynePacket().data); + static const size_t packet_size = + sizeof(velodyne_msgs::VelodynePacket().data); + + //////////////////////////////////////////////////////////////////////// + // Input base class implementation + //////////////////////////////////////////////////////////////////////// + + /** @brief constructor + * + * @param private_nh ROS private handle for calling node. + * @param port UDP port number. + */ + Input::Input(ros::NodeHandle private_nh, int port): + private_nh_(private_nh), + port_(port) + { + private_nh.param("device_ip", devip_str_, std::string("")); + if (!devip_str_.empty()) + ROS_INFO_STREAM("Only accepting packets from IP address: " + << devip_str_); + } //////////////////////////////////////////////////////////////////////// // InputSocket class implementation @@ -40,16 +63,16 @@ namespace velodyne_driver /** @brief constructor * - * @param private_nh private node handle for driver - * @param udp_port UDP port number to connect + * @param private_nh ROS private handle for calling node. + * @param port UDP port number */ - InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t udp_port): - Input() + InputSocket::InputSocket(ros::NodeHandle private_nh, int port): + Input(private_nh, port) { sockfd_ = -1; // connect to Velodyne UDP port - ROS_INFO_STREAM("Opening UDP socket: port " << udp_port); + ROS_INFO_STREAM("Opening UDP socket: port " << port); sockfd_ = socket(PF_INET, SOCK_DGRAM, 0); if (sockfd_ == -1) { @@ -60,7 +83,7 @@ namespace velodyne_driver sockaddr_in my_addr; // my address information memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros my_addr.sin_family = AF_INET; // host byte order - my_addr.sin_port = htons(udp_port); // short, in network byte order + my_addr.sin_port = htons(port); // port in network byte order my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP if (bind(sockfd_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1) @@ -84,12 +107,6 @@ namespace velodyne_driver (void) close(sockfd_); } - void InputSocket::setDeviceIP(const std::string &ip) - { - devip_str_ = ip; - inet_aton(ip.c_str(),&devip_); - } - /** @brief Get one velodyne packet. */ int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt) { @@ -150,7 +167,8 @@ namespace velodyne_driver // socket using a blocking read. ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], packet_size, 0, - (sockaddr*) &sender_address, &sender_address_len); + (sockaddr*) &sender_address, + &sender_address_len); if (nbytes < 0) { @@ -166,7 +184,8 @@ namespace velodyne_driver // read successful, // if packet is not from the lidar scanner we selected by IP, // continue otherwise we are done - if( devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr ) + if(devip_str_ != "" + && sender_address.sin_addr.s_addr != devip_.s_addr) continue; else break; //done @@ -190,31 +209,24 @@ namespace velodyne_driver /** @brief constructor * - * @param private_nh private node handle for driver + * @param private_nh ROS private handle for calling node. + * @param port UDP port number * @param packet_rate expected device packet frequency (Hz) * @param filename PCAP dump file name - * @param read_once read PCAP in a loop, unless false - * @param read_fast read PCAP at device rate, unless false - * @param repeat_delay time to wait before repeating PCAP data */ - InputPCAP::InputPCAP(ros::NodeHandle private_nh, - double packet_rate, - std::string filename, - bool read_once, - bool read_fast, - double repeat_delay): - Input(), - packet_rate_(packet_rate) + InputPCAP::InputPCAP(ros::NodeHandle private_nh, int port, + double packet_rate, std::string filename): + Input(private_nh, port), + packet_rate_(packet_rate), + filename_(filename) { - filename_ = filename; - fp_ = NULL; pcap_ = NULL; empty_ = true; // get parameters using private node handle - private_nh.param("read_once", read_once_, read_once); - private_nh.param("read_fast", read_fast_, read_fast); - private_nh.param("repeat_delay", repeat_delay_, repeat_delay); + private_nh.param("read_once", read_once_, false); + private_nh.param("read_fast", read_fast_, false); + private_nh.param("repeat_delay", repeat_delay_, 0.0); if (read_once_) ROS_INFO("Read input file only once."); @@ -231,8 +243,16 @@ namespace velodyne_driver ROS_FATAL("Error opening Velodyne socket dump file."); return; } - } + std::stringstream filter; + if( devip_str_ != "" ) // using specific IP? + { + filter << "src host " << devip_str_ << " && "; + } + filter << "udp dst port " << port; + pcap_compile(pcap_, &pcap_packet_filter_, + filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN); + } /** destructor */ InputPCAP::~InputPCAP(void) @@ -240,13 +260,6 @@ namespace velodyne_driver pcap_close(pcap_); } - void InputPCAP::setDeviceIP(const std::string &ip) - { - std::string filter_str = "src host " + devip_str_ + " && udp src port 2368 && udp dst port 2368"; - if( devip_str_ != "" ) - pcap_compile(pcap_, &velodyne_pointdata_filter_, filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN); - } - /** @brief Get one velodyne packet. */ int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt) { @@ -258,8 +271,11 @@ namespace velodyne_driver int res; if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) { - // if packet is not from the lidar scanner we selected by IP, continue - if( !devip_str_.empty() && (pcap_offline_filter( &velodyne_pointdata_filter_, header, pkt_data ) == 0) ) + // Skip packets not for the correct port and from the + // selected IP address. + if (!devip_str_.empty() && + (0 == pcap_offline_filter(&pcap_packet_filter_, + header, pkt_data))) continue; // Keep the reader from blowing through the file. From f75228e44aada8203024e994b230e9e6fd627e22 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 29 Nov 2015 14:47:31 +0100 Subject: [PATCH 09/66] Remove unused variable I think that `dsr` was unused. See line 317: for (int dsr=0; ... --- velodyne_pointcloud/src/lib/rawdata.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index c8d96b6a5..78a16bd71 100755 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -290,8 +290,7 @@ namespace velodyne_rawdata int azimuth_corrected; float x, y, z; float intensity; - uint8_t dsr; - + const raw_packet_t *raw = (const raw_packet_t *) &pkt.data[0]; for (int block = 0; block < BLOCKS_PER_PACKET; block++) { From 0c235c135aa4c72ed8de2538014c99d419e01706 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Mon, 8 Feb 2016 10:55:41 +0100 Subject: [PATCH 10/66] Fix and add a few comments. --- velodyne_pointcloud/src/lib/rawdata.cc | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index 78a16bd71..3060bfdbd 100755 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -44,7 +44,7 @@ namespace velodyne_rawdata RawData::RawData() {} - /** Uppdate parameters: conversions and update */ + /** Update parameters: conversions and update */ void RawData::setParameters(double min_range, double max_range, double view_direction, @@ -305,6 +305,7 @@ namespace velodyne_rawdata return; // bad packet: skip the rest } + // Calculate difference between current and next block's azimuth angle. azimuth = (float)(raw->blocks[block].rotation); if (block < (BLOCKS_PER_PACKET-1)){ azimuth_diff = (float)((36000 + raw->blocks[block+1].rotation - raw->blocks[block].rotation)%36000); @@ -335,6 +336,8 @@ namespace velodyne_rawdata ||(config_.min_angle > config_.max_angle && (azimuth_corrected <= config_.max_angle || azimuth_corrected >= config_.min_angle))){ + + // convert polar coordinates to Euclidean XYZ float distance = tmp.uint * DISTANCE_RESOLUTION; distance += corrections.dist_correction; @@ -418,7 +421,6 @@ namespace velodyne_rawdata float z_coord = z; /** Intensity Calculation */ - float min_intensity = corrections.min_intensity; float max_intensity = corrections.max_intensity; @@ -435,15 +437,14 @@ namespace velodyne_rawdata if (pointInRange(distance)) { - // convert polar coordinates to Euclidean XYZ + // append this point to the cloud VPoint point; point.ring = corrections.laser_ring; point.x = x_coord; point.y = y_coord; point.z = z_coord; point.intensity = (uint8_t) intensity; - - // append this point to the cloud + pc.points.push_back(point); ++pc.width; } From 597aa9123f8141779259a2cd999e5fd1c512db96 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Tue, 9 Feb 2016 16:04:11 +0100 Subject: [PATCH 11/66] Fix data type error that distorts the point cloud. --- .../include/velodyne_pointcloud/rawdata.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index 3dfbe8f64..31100acbc 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h @@ -61,11 +61,11 @@ namespace velodyne_rawdata /** Special Defines for VLP16 support **/ - static const int VLP16_FIRINGS_PER_BLOCK = 2; - static const int VLP16_SCANS_PER_FIRING = 16; - static const int VLP16_BLOCK_TDURATION = 110.592; - static const int VLP16_DSR_TOFFSET = 2.304; - static const int VLP16_FIRING_TOFFSET = 55.296; + static const int VLP16_FIRINGS_PER_BLOCK = 2; + static const int VLP16_SCANS_PER_FIRING = 16; + static const float VLP16_BLOCK_TDURATION = 110.592f; // [µs] + static const float VLP16_DSR_TOFFSET = 2.304f; // [µs] + static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs] /** \brief Raw Velodyne data block. From 3f6924e732445e7e5e1cfab787b71eb5831e0e17 Mon Sep 17 00:00:00 2001 From: pomerlef Date: Thu, 11 Feb 2016 18:11:21 -0500 Subject: [PATCH 12/66] resolve sign error --- velodyne_pointcloud/src/lib/rawdata.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index 78a16bd71..2f5515c2a 100755 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -183,7 +183,7 @@ namespace velodyne_rawdata * was added to the expression due to the mathemathical * model we used. */ - float xy_distance = distance * cos_vert_angle + vert_offset * sin_vert_angle; + float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle; // Calculate temporal X, use absolute value. float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle; @@ -215,12 +215,12 @@ namespace velodyne_rawdata * was added to the expression due to the mathemathical * model we used. */ - xy_distance = distance_x * cos_vert_angle + vert_offset * sin_vert_angle ; + xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle ; ///the expression wiht '-' is proved to be better than the one with '+' x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle; float distance_y = distance + distance_corr_y; - xy_distance = distance_y * cos_vert_angle + vert_offset * sin_vert_angle ; + xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle ; /**the new term of 'vert_offset * sin_vert_angle' * was added to the expression due to the mathemathical * model we used. @@ -360,7 +360,7 @@ namespace velodyne_rawdata * was added to the expression due to the mathemathical * model we used. */ - float xy_distance = distance * cos_vert_angle + vert_offset * sin_vert_angle; + float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle; // Calculate temporal X, use absolute value. float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle; @@ -392,7 +392,7 @@ namespace velodyne_rawdata * was added to the expression due to the mathemathical * model we used. */ - xy_distance = distance_x * cos_vert_angle + vert_offset * sin_vert_angle ; + xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle ; x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle; float distance_y = distance + distance_corr_y; @@ -400,7 +400,7 @@ namespace velodyne_rawdata * was added to the expression due to the mathemathical * model we used. */ - xy_distance = distance_y * cos_vert_angle + vert_offset * sin_vert_angle ; + xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle ; y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle; // Using distance_y is not symmetric, but the velodyne manual From 03db2fd3c644ce1fac8d0f216460b1df0304f84c Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Wed, 17 Feb 2016 11:58:36 +0100 Subject: [PATCH 13/66] Add *.user files to gitignore. --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index d53a461ce..9c8611737 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ *.pyc *.tar.gz *~ +*.user TAGS From 796d1395318d66a3788309ec41a8ddaab8b4d025 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Wed, 17 Feb 2016 11:59:40 +0100 Subject: [PATCH 14/66] Align console output of calibration data. --- velodyne_pointcloud/src/lib/calibration.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/velodyne_pointcloud/src/lib/calibration.cc b/velodyne_pointcloud/src/lib/calibration.cc index e40c828f6..5f2ae85c5 100644 --- a/velodyne_pointcloud/src/lib/calibration.cc +++ b/velodyne_pointcloud/src/lib/calibration.cc @@ -145,8 +145,8 @@ namespace velodyne_pointcloud // store this ring number with its corresponding laser number calibration.laser_corrections[next_index].laser_ring = ring; next_angle = min_seen; - ROS_INFO_STREAM("laser_ring[" << next_index << "] = " << ring - << ", angle = " << next_angle); + ROS_INFO("laser_ring[%2u] = %2u, angle = %+.6f", + next_index, ring, next_angle); } } } From acce5e1ffffd8968de73060d5c86e1fc19e42494 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Thu, 18 Feb 2016 12:09:06 +0100 Subject: [PATCH 15/66] Fix line that always indicates use of model VLP-16. --- velodyne_pointcloud/src/lib/rawdata.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index 3060bfdbd..65447400e 100755 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -95,7 +95,7 @@ namespace velodyne_rawdata return -1; } - ROS_INFO_STREAM("Data will be processed as a VLP-16...num_lasers: " << calibration_.num_lasers); + ROS_INFO_STREAM("Number of lasers: " << calibration_.num_lasers << "."); // Set up cached values for sin and cos of all the possible headings for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { From 285618053eb8788af9cd0a8a1f184f52fbf14af3 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Thu, 18 Feb 2016 17:25:34 +0100 Subject: [PATCH 16/66] Add a missing space. --- velodyne_pointcloud/src/conversions/transform.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index a936086d7..8833850f2 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -90,7 +90,7 @@ namespace velodyne_pointcloud // transform the packet point cloud into the target frame try { - ROS_DEBUG_STREAM("transforming from" << inPc_.header.frame_id + ROS_DEBUG_STREAM("transforming from " << inPc_.header.frame_id << " to " << config_.frame_id); pcl_ros::transformPointCloud(config_.frame_id, inPc_, tfPc_, listener_); From 80a1494215dd1807f8d8315e332005bae3b59dc6 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Fri, 19 Feb 2016 18:21:11 +0100 Subject: [PATCH 17/66] Add docs to gitignore. Documentation tools create this folder that shall not be uploaded to git. --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 9c8611737..da9cbf726 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,5 @@ *.tar.gz *~ *.user +doc TAGS From 26fface987ee1c25d6c68beb3f947950a619e492 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Fri, 19 Feb 2016 19:07:41 +0100 Subject: [PATCH 18/66] Set up dynamic reconfiguration for transform_node. Previously, transform_node has neither read parameters other than frame_id from the command line nor has it exposed these parameters via dynamic reconfigure. As parameters like max_range and view_width have been initialized to zero, the inconfigurable transform_node has returned an empty point cloud. Now, transform_node launches an reconfigure server just as cloud_node. In contrast to cloud_node, transform node exposes another parameter for dynamic reconfiguration: frame_id, i.e. the frame of reference the incoming Velodyne points are transformed to. --- velodyne_pointcloud/CMakeLists.txt | 11 ++++++++--- .../cfg/{VelodyneConfig.cfg => CloudNode.cfg} | 2 +- velodyne_pointcloud/cfg/TransformNode.cfg | 17 +++++++++++++++++ .../src/conversions/CMakeLists.txt | 4 ++++ velodyne_pointcloud/src/conversions/convert.cc | 6 +++--- velodyne_pointcloud/src/conversions/convert.h | 6 +++--- .../src/conversions/transform.cc | 16 ++++++++++++++++ velodyne_pointcloud/src/conversions/transform.h | 9 +++++++++ 8 files changed, 61 insertions(+), 10 deletions(-) rename velodyne_pointcloud/cfg/{VelodyneConfig.cfg => CloudNode.cfg} (87%) create mode 100755 velodyne_pointcloud/cfg/TransformNode.cfg diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index cc3883196..6a368ffec 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -10,7 +10,9 @@ set(${PROJECT_NAME}_CATKIN_DEPS sensor_msgs tf velodyne_driver - velodyne_msgs) + velodyne_msgs + dynamic_reconfigure +) find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_DEPS} pcl_conversions) @@ -28,13 +30,16 @@ find_library(YAML_CPP_LIBRARY PATHS ${YAML_CPP_LIBRARY_DIRS}) generate_dynamic_reconfigure_options( - cfg/VelodyneConfig.cfg) + cfg/CloudNode.cfg cfg/TransformNode.cfg +) if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") add_definitions(-DHAVE_NEW_YAMLCPP) endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} + ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake +) catkin_package( CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} diff --git a/velodyne_pointcloud/cfg/VelodyneConfig.cfg b/velodyne_pointcloud/cfg/CloudNode.cfg similarity index 87% rename from velodyne_pointcloud/cfg/VelodyneConfig.cfg rename to velodyne_pointcloud/cfg/CloudNode.cfg index 57493162b..c4f45c783 100755 --- a/velodyne_pointcloud/cfg/VelodyneConfig.cfg +++ b/velodyne_pointcloud/cfg/CloudNode.cfg @@ -13,4 +13,4 @@ gen.add("view_direction", double_t, 0, "angle defining the center of view", gen.add("view_width", double_t, 0, "angle defining the view width", 2*pi, 0.0, 2*pi) -exit(gen.generate(PACKAGE, "velodyne_pointcloud", "VelodyneConfig")) +exit(gen.generate(PACKAGE, "cloud_node", "CloudNode")) diff --git a/velodyne_pointcloud/cfg/TransformNode.cfg b/velodyne_pointcloud/cfg/TransformNode.cfg new file mode 100755 index 000000000..b21d5c20d --- /dev/null +++ b/velodyne_pointcloud/cfg/TransformNode.cfg @@ -0,0 +1,17 @@ +#!/usr/bin/env python +PACKAGE = "velodyne_pointcloud" + +from math import pi +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.1, 10.0) +gen.add("max_range", double_t, 0, "max range to publish", 130, 0.1, 200) +gen.add("view_direction", double_t, 0, "angle defining the center of view", + 0.0, -pi, pi) +gen.add("view_width", double_t, 0, "angle defining the view width", + 2*pi, 0.0, 2*pi) +gen.add("frame_id", str_t, 0, "new frame of reference for point clouds", "odom") + +exit(gen.generate(PACKAGE, "transform_node", "TransformNode")) diff --git a/velodyne_pointcloud/src/conversions/CMakeLists.txt b/velodyne_pointcloud/src/conversions/CMakeLists.txt index 15568e3de..2d149531e 100644 --- a/velodyne_pointcloud/src/conversions/CMakeLists.txt +++ b/velodyne_pointcloud/src/conversions/CMakeLists.txt @@ -1,10 +1,12 @@ add_executable(cloud_node cloud_node.cc convert.cc) +add_dependencies(cloud_node ${PROJECT_NAME}_gencfg) target_link_libraries(cloud_node velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS cloud_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) add_library(cloud_nodelet cloud_nodelet.cc convert.cc) +add_dependencies(cloud_nodelet ${PROJECT_NAME}_gencfg) target_link_libraries(cloud_nodelet velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS cloud_nodelet @@ -27,12 +29,14 @@ install(TARGETS ringcolors_nodelet LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) add_executable(transform_node transform_node.cc transform.cc) +add_dependencies(transform_node ${PROJECT_NAME}_gencfg) target_link_libraries(transform_node velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS transform_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) add_library(transform_nodelet transform_nodelet.cc transform.cc) +add_dependencies(transform_nodelet ${PROJECT_NAME}_gencfg) target_link_libraries(transform_nodelet velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS transform_nodelet diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc index 12730925f..165a75297 100644 --- a/velodyne_pointcloud/src/conversions/convert.cc +++ b/velodyne_pointcloud/src/conversions/convert.cc @@ -31,8 +31,8 @@ namespace velodyne_pointcloud node.advertise("velodyne_points", 10); srv_ = boost::make_shared > (private_nh); - dynamic_reconfigure::Server:: + CloudNodeConfig> > (private_nh); + dynamic_reconfigure::Server:: CallbackType f; f = boost::bind (&Convert::callback, this, _1, _2); srv_->setCallback (f); @@ -44,7 +44,7 @@ namespace velodyne_pointcloud ros::TransportHints().tcpNoDelay(true)); } - void Convert::callback(velodyne_pointcloud::VelodyneConfigConfig &config, + void Convert::callback(velodyne_pointcloud::CloudNodeConfig &config, uint32_t level) { ROS_INFO("Reconfigure Request"); diff --git a/velodyne_pointcloud/src/conversions/convert.h b/velodyne_pointcloud/src/conversions/convert.h index 7886f4269..953a6cac6 100644 --- a/velodyne_pointcloud/src/conversions/convert.h +++ b/velodyne_pointcloud/src/conversions/convert.h @@ -23,7 +23,7 @@ #include #include -#include +#include namespace velodyne_pointcloud { @@ -36,13 +36,13 @@ namespace velodyne_pointcloud private: - void callback(velodyne_pointcloud::VelodyneConfigConfig &config, + void callback(velodyne_pointcloud::CloudNodeConfig &config, uint32_t level); void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg); ///Pointer to dynamic reconfigure service srv_ boost::shared_ptr > srv_; + CloudNodeConfig> > srv_; boost::shared_ptr data_; ros::Subscriber velodyne_scan_; diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 8833850f2..1b9c8249d 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -38,6 +38,13 @@ namespace velodyne_pointcloud output_ = node.advertise("velodyne_points", 10); + srv_ = boost::make_shared > (private_nh); + dynamic_reconfigure::Server:: + CallbackType f; + f = boost::bind (&Transform::callback, this, _1, _2); + srv_->setCallback (f); + // subscribe to VelodyneScan packets using transform filter velodyne_scan_.subscribe(node, "velodyne_packets", 10); tf_filter_ = @@ -46,6 +53,15 @@ namespace velodyne_pointcloud config_.frame_id, 10); tf_filter_->registerCallback(boost::bind(&Transform::processScan, this, _1)); } + + void Transform::callback(velodyne_pointcloud::TransformNodeConfig &config, + uint32_t level) + { + ROS_INFO("Reconfigure Request"); + data_->setParameters(config.min_range, config.max_range, + config.view_direction, config.view_width); + config_.frame_id = config.frame_id; + } /** @brief Callback for raw scan messages. * diff --git a/velodyne_pointcloud/src/conversions/transform.h b/velodyne_pointcloud/src/conversions/transform.h index 734f1d49a..676c63ef0 100644 --- a/velodyne_pointcloud/src/conversions/transform.h +++ b/velodyne_pointcloud/src/conversions/transform.h @@ -26,6 +26,9 @@ #include #include +#include +#include + // include template implementations to transform a custom point cloud #include @@ -53,6 +56,12 @@ namespace velodyne_pointcloud void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg); + ///Pointer to dynamic reconfigure service srv_ + boost::shared_ptr > srv_; + void callback(velodyne_pointcloud::TransformNodeConfig &config, + uint32_t level); + boost::shared_ptr data_; message_filters::Subscriber velodyne_scan_; tf::MessageFilter *tf_filter_; From 95b46918ac8d236f5b3af14e42b278e8bb832ed8 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Mon, 22 Feb 2016 09:30:37 +0100 Subject: [PATCH 19/66] Improve coding style. --- velodyne_pointcloud/cfg/TransformNode.cfg | 40 ++++++++++++++++++----- 1 file changed, 31 insertions(+), 9 deletions(-) diff --git a/velodyne_pointcloud/cfg/TransformNode.cfg b/velodyne_pointcloud/cfg/TransformNode.cfg index b21d5c20d..e6948b9b7 100755 --- a/velodyne_pointcloud/cfg/TransformNode.cfg +++ b/velodyne_pointcloud/cfg/TransformNode.cfg @@ -2,16 +2,38 @@ PACKAGE = "velodyne_pointcloud" from math import pi -from dynamic_reconfigure.parameter_generator_catkin import * +import dynamic_reconfigure.parameter_generator_catkin as pgc -gen = ParameterGenerator() +gen = pgc.ParameterGenerator() -gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.1, 10.0) -gen.add("max_range", double_t, 0, "max range to publish", 130, 0.1, 200) -gen.add("view_direction", double_t, 0, "angle defining the center of view", - 0.0, -pi, pi) -gen.add("view_width", double_t, 0, "angle defining the view width", - 2*pi, 0.0, 2*pi) -gen.add("frame_id", str_t, 0, "new frame of reference for point clouds", "odom") +gen.add("min_range", + pgc.double_t, + 0, + "min range to publish", + 0.9, 0.1, 10.0) + +gen.add("max_range", + pgc.double_t, + 0, + "max range to publish", + 130, 0.1, 200) + +gen.add("view_direction", + pgc.double_t, + 0, + "angle defining the center of view", + 0.0, -pi, pi) + +gen.add("view_width", + pgc.double_t, + 0, + "angle defining the view width", + 2*pi, 0.0, 2*pi) + +gen.add("frame_id", + pgc.str_t, + 0, + "new frame of reference for point clouds", + "odom") exit(gen.generate(PACKAGE, "transform_node", "TransformNode")) From f7d0ce489212ed639dee9efac68fa6563eab1f57 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Mon, 22 Feb 2016 10:20:55 +0100 Subject: [PATCH 20/66] Resolve frame ID name using tf prefix. --- .../src/conversions/transform.cc | 20 +++++++++---------- .../src/conversions/transform.h | 3 ++- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 1b9c8249d..4bf5a2e99 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -25,13 +25,10 @@ namespace velodyne_pointcloud { /** @brief Constructor. */ Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh): - data_(new velodyne_rawdata::RawData()) + data_(new velodyne_rawdata::RawData()), + tf_prefix_(tf::getPrefixParam(private_nh)) { - private_nh.param("frame_id", config_.frame_id, std::string("odom")); - std::string tf_prefix = tf::getPrefixParam(private_nh); - config_.frame_id = tf::resolve(tf_prefix, config_.frame_id); - ROS_INFO_STREAM("target frame ID: " << config_.frame_id); - + // Read calibration. data_->setup(private_nh); // advertise output point cloud (before subscribing to input data) @@ -42,7 +39,7 @@ namespace velodyne_pointcloud TransformNodeConfig> > (private_nh); dynamic_reconfigure::Server:: CallbackType f; - f = boost::bind (&Transform::callback, this, _1, _2); + f = boost::bind (&Transform::reconfigure_callback, this, _1, _2); srv_->setCallback (f); // subscribe to VelodyneScan packets using transform filter @@ -54,13 +51,14 @@ namespace velodyne_pointcloud tf_filter_->registerCallback(boost::bind(&Transform::processScan, this, _1)); } - void Transform::callback(velodyne_pointcloud::TransformNodeConfig &config, - uint32_t level) + void Transform::reconfigure_callback( + velodyne_pointcloud::TransformNodeConfig &config, uint32_t level) { - ROS_INFO("Reconfigure Request"); + ROS_INFO_STREAM("Reconfigure request."); data_->setParameters(config.min_range, config.max_range, config.view_direction, config.view_width); - config_.frame_id = config.frame_id; + config_.frame_id = tf::resolve(tf_prefix_, config.frame_id); + ROS_INFO_STREAM("Target frame ID: " << config_.frame_id); } /** @brief Callback for raw scan messages. diff --git a/velodyne_pointcloud/src/conversions/transform.h b/velodyne_pointcloud/src/conversions/transform.h index 676c63ef0..3ee0c3f7d 100644 --- a/velodyne_pointcloud/src/conversions/transform.h +++ b/velodyne_pointcloud/src/conversions/transform.h @@ -59,9 +59,10 @@ namespace velodyne_pointcloud ///Pointer to dynamic reconfigure service srv_ boost::shared_ptr > srv_; - void callback(velodyne_pointcloud::TransformNodeConfig &config, + void reconfigure_callback(velodyne_pointcloud::TransformNodeConfig &config, uint32_t level); + const std::string tf_prefix_; boost::shared_ptr data_; message_filters::Subscriber velodyne_scan_; tf::MessageFilter *tf_filter_; From 47df6917bc944a000fe133e86962edf8047d5b47 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 8 Mar 2016 19:17:46 -0800 Subject: [PATCH 21/66] allow floats instead of ints in min/max_intensity --- velodyne_pointcloud/src/lib/calibration.cc | 43 +++++++++++++++++----- 1 file changed, 34 insertions(+), 9 deletions(-) diff --git a/velodyne_pointcloud/src/lib/calibration.cc b/velodyne_pointcloud/src/lib/calibration.cc index 5f2ae85c5..43714badb 100644 --- a/velodyne_pointcloud/src/lib/calibration.cc +++ b/velodyne_pointcloud/src/lib/calibration.cc @@ -75,23 +75,48 @@ namespace velodyne_pointcloud node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction; node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction; #ifdef HAVE_NEW_YAMLCPP - if (node[MAX_INTENSITY]) - node[MAX_INTENSITY] >> correction.second.max_intensity; #else - if (const YAML::Node *pName = node.FindValue(MAX_INTENSITY)) - *pName >> correction.second.max_intensity; #endif else + const YAML::Node * max_intensity_node; +#ifdef HAVE_NEW_YAMLCPP + max_intensity_node = node[MAX_INTENSITY]; +#else + max_intensity_node = node.FindValue(MAX_INTENSITY); +#endif + if (max_intensity_node) { + try { + *max_intensity_node >> correction.second.max_intensity; + } catch(const YAML::InvalidScalar & exc) { + float max_intensity_float; + *max_intensity_node >> max_intensity_float; + correction.second.max_intensity = floor(max_intensity_float); + ROS_WARN_ONCE("Implicitly converting 'max_intensity' values from floats to ints."); + } + } + else { correction.second.max_intensity = 255; + } + + const YAML::Node * min_intensity_node; #ifdef HAVE_NEW_YAMLCPP - if (node[MIN_INTENSITY]) - node[MIN_INTENSITY] >> correction.second.min_intensity; + min_intensity_node = node[MIN_INTENSITY]; #else - if (const YAML::Node *pName = node.FindValue(MIN_INTENSITY)) - *pName >> correction.second.min_intensity; + min_intensity_node = node.FindValue(MIN_INTENSITY); #endif - else + if (min_intensity_node) { + try { + *min_intensity_node >> correction.second.min_intensity; + } catch(const YAML::InvalidScalar & exc) { + float min_intensity_float; + *min_intensity_node >> min_intensity_float; + correction.second.min_intensity = floor(min_intensity_float); + ROS_WARN_ONCE("Implicitly converting 'min_intensity' values from floats to ints."); + } + } + else { correction.second.min_intensity = 0; + } node[FOCAL_DISTANCE] >> correction.second.focal_distance; node[FOCAL_SLOPE] >> correction.second.focal_slope; From ccb9a0b45df29db0c78321988090f268e93c15bb Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 8 Mar 2016 19:18:21 -0800 Subject: [PATCH 22/66] allow horiz_offset_correction to be optional with 0 as default --- velodyne_pointcloud/src/lib/calibration.cc | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/velodyne_pointcloud/src/lib/calibration.cc b/velodyne_pointcloud/src/lib/calibration.cc index 43714badb..c7e6aa307 100644 --- a/velodyne_pointcloud/src/lib/calibration.cc +++ b/velodyne_pointcloud/src/lib/calibration.cc @@ -73,11 +73,17 @@ namespace velodyne_pointcloud node[DIST_CORRECTION_X] >> correction.second.dist_correction_x; node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y; node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction; - node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction; #ifdef HAVE_NEW_YAMLCPP + if (node[HORIZ_OFFSET_CORRECTION]) + node[HORIZ_OFFSET_CORRECTION] >> + correction.second.horiz_offset_correction; #else + if (const YAML::Node *pName = node.FindValue(HORIZ_OFFSET_CORRECTION)) + *pName >> correction.second.horiz_offset_correction; #endif else + correction.second.horiz_offset_correction = 0; + const YAML::Node * max_intensity_node; #ifdef HAVE_NEW_YAMLCPP max_intensity_node = node[MAX_INTENSITY]; From bf1592c222634d6ae6b7e84e2369ed8fe1307c6a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Mar 2016 19:04:39 -0800 Subject: [PATCH 23/66] add travis pr testing --- .travis.yml | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 000000000..e5ebfafb8 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,35 @@ +sudo: required +dist: trusty +# Force travis to use its minimal image with default Python settings +language: generic +compiler: + - gcc +env: + global: + - CATKIN_WS=~/catkin_ws + - CATKIN_WS_SRC=${CATKIN_WS}/src + matrix: + - CI_ROS_DISTRO="indigo" + - CI_ROS_DISTRO="jade" +install: + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update -qq + - sudo apt-get install -qq -y python-rosdep + - sudo rosdep init + - rosdep update + # Use rosdep to install all dependencies (including ROS itself) + - rosdep install --from-paths ./ -i -y --rosdistro $CI_ROS_DISTRO +script: + - source /opt/ros/$CI_ROS_DISTRO/setup.bash + - mkdir -p $CATKIN_WS_SRC + - ln -s $TRAVIS_BUILD_DIR $CATKIN_WS_SRC + - cd $CATKIN_WS + # Build [and Install] packages + - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release + # Build tests + - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args tests + # Run tests + - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args run_tests + # Check results + - catkin_test_results ./build From 5b1ecedf8ae70824f9e303f69fa6ce355a9f3ede Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Mar 2016 19:14:38 -0800 Subject: [PATCH 24/66] make a build failure stop the next steps --- .travis.yml | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/.travis.yml b/.travis.yml index e5ebfafb8..6117e5a16 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,11 +25,9 @@ script: - mkdir -p $CATKIN_WS_SRC - ln -s $TRAVIS_BUILD_DIR $CATKIN_WS_SRC - cd $CATKIN_WS - # Build [and Install] packages - - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release - # Build tests - - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args tests - # Run tests - - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args run_tests + # Build [and Install] packages, build tests, and run tests + - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args tests && \ + catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args run_tests # Check results - catkin_test_results ./build From 0b82b20fa59f21942a02259286291a76ff077528 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Mar 2016 18:51:18 -0800 Subject: [PATCH 25/66] fix the yaml-cpp 0.5 code paths --- velodyne_pointcloud/CMakeLists.txt | 4 +++- velodyne_pointcloud/src/lib/calibration.cc | 6 ++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index cc3883196..0b699951e 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -26,7 +26,9 @@ find_path(YAML_CPP_INCLUDE_DIR find_library(YAML_CPP_LIBRARY NAMES YAML_CPP PATHS ${YAML_CPP_LIBRARY_DIRS}) - + +link_directories(${YAML_CPP_LIBRARY_DIRS}) + generate_dynamic_reconfigure_options( cfg/VelodyneConfig.cfg) diff --git a/velodyne_pointcloud/src/lib/calibration.cc b/velodyne_pointcloud/src/lib/calibration.cc index c7e6aa307..01c6112e5 100644 --- a/velodyne_pointcloud/src/lib/calibration.cc +++ b/velodyne_pointcloud/src/lib/calibration.cc @@ -86,7 +86,8 @@ namespace velodyne_pointcloud const YAML::Node * max_intensity_node; #ifdef HAVE_NEW_YAMLCPP - max_intensity_node = node[MAX_INTENSITY]; + const YAML::Node max_intensity_node_ref = node[MAX_INTENSITY]; + max_intensity_node = &max_intensity_node_ref; #else max_intensity_node = node.FindValue(MAX_INTENSITY); #endif @@ -106,7 +107,8 @@ namespace velodyne_pointcloud const YAML::Node * min_intensity_node; #ifdef HAVE_NEW_YAMLCPP - min_intensity_node = node[MIN_INTENSITY]; + const YAML::Node min_intensity_node_ref = node[MIN_INTENSITY]; + min_intensity_node = &min_intensity_node_ref; #else min_intensity_node = node.FindValue(MIN_INTENSITY); #endif From f3f1fdecbe31b15c21561cedb0a885738103c30a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Mar 2016 19:35:03 -0800 Subject: [PATCH 26/66] try to fixup travis-ci --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 6117e5a16..aa08bf390 100644 --- a/.travis.yml +++ b/.travis.yml @@ -26,8 +26,8 @@ script: - ln -s $TRAVIS_BUILD_DIR $CATKIN_WS_SRC - cd $CATKIN_WS # Build [and Install] packages, build tests, and run tests - - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release && \ - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args tests && \ + - catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release && + catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args tests && catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args run_tests # Check results - catkin_test_results ./build From 594d0be6e8b1c29f953ed45007b5c4b2c7a7057a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 11 Mar 2016 19:48:17 -0800 Subject: [PATCH 27/66] fix test_results call --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index aa08bf390..648538765 100644 --- a/.travis.yml +++ b/.travis.yml @@ -30,4 +30,4 @@ script: catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args tests && catkin_make_isolated --install --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-make-args run_tests # Check results - - catkin_test_results ./build + - catkin_test_results ./build_isolated From fbe603a5cb0e74cf75aa78859c08910197b740b4 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Sun, 20 Mar 2016 18:42:49 -0500 Subject: [PATCH 28/66] calibration: make max_intensity and min_intensity optional (#84) This fixes a regression in the 32e and VLP-16 calibrations which do not contain intensity values. There is still a problem with the 64e_s2.1 calibration. --- velodyne_pointcloud/src/lib/calibration.cc | 24 ++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/velodyne_pointcloud/src/lib/calibration.cc b/velodyne_pointcloud/src/lib/calibration.cc index 01c6112e5..e87e0c437 100644 --- a/velodyne_pointcloud/src/lib/calibration.cc +++ b/velodyne_pointcloud/src/lib/calibration.cc @@ -53,6 +53,7 @@ namespace velodyne_pointcloud const std::string FOCAL_DISTANCE = "focal_distance"; const std::string FOCAL_SLOPE = "focal_slope"; + /** Read calibration for a single laser. */ void operator >> (const YAML::Node& node, std::pair& correction) { @@ -84,12 +85,15 @@ namespace velodyne_pointcloud else correction.second.horiz_offset_correction = 0; - const YAML::Node * max_intensity_node; + const YAML::Node * max_intensity_node = NULL; #ifdef HAVE_NEW_YAMLCPP - const YAML::Node max_intensity_node_ref = node[MAX_INTENSITY]; - max_intensity_node = &max_intensity_node_ref; + if (node[MAX_INTENSITY]) { + const YAML::Node max_intensity_node_ref = node[MAX_INTENSITY]; + max_intensity_node = &max_intensity_node_ref; + } #else - max_intensity_node = node.FindValue(MAX_INTENSITY); + if (const YAML::Node *pName = node.FindValue(MAX_INTENSITY)) + max_intensity_node = pName; #endif if (max_intensity_node) { try { @@ -105,12 +109,15 @@ namespace velodyne_pointcloud correction.second.max_intensity = 255; } - const YAML::Node * min_intensity_node; + const YAML::Node * min_intensity_node = NULL; #ifdef HAVE_NEW_YAMLCPP - const YAML::Node min_intensity_node_ref = node[MIN_INTENSITY]; - min_intensity_node = &min_intensity_node_ref; + if (node[MIN_INTENSITY]) { + const YAML::Node min_intensity_node_ref = node[MIN_INTENSITY]; + min_intensity_node = &min_intensity_node_ref; + } #else - min_intensity_node = node.FindValue(MIN_INTENSITY); + if (const YAML::Node *pName = node.FindValue(MIN_INTENSITY)) + min_intensity_node = pName; #endif if (min_intensity_node) { try { @@ -141,6 +148,7 @@ namespace velodyne_pointcloud correction.second.laser_ring = 0; // clear initially (set later) } + /** Read entire calibration file. */ void operator >> (const YAML::Node& node, Calibration& calibration) { int num_lasers; From c3e5178aca169f37bc60e87d02aaa2bbb14e13fa Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Mon, 21 Mar 2016 11:00:27 -0500 Subject: [PATCH 29/66] calibration: add gtest for #84 This currently fails on 64e_s2.1-sztaki.yaml and on issue_84_float_intensities.yaml. --- velodyne_pointcloud/CMakeLists.txt | 49 +--- velodyne_pointcloud/tests/CMakeLists.txt | 53 ++++ .../tests/issue_84_float_intensities.yaml | 239 ++++++++++++++++++ .../tests/test_calibration.cpp | 69 +++++ 4 files changed, 363 insertions(+), 47 deletions(-) create mode 100644 velodyne_pointcloud/tests/CMakeLists.txt create mode 100644 velodyne_pointcloud/tests/issue_84_float_intensities.yaml create mode 100644 velodyne_pointcloud/tests/test_calibration.cpp diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index 0b699951e..1865c7c1b 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -65,50 +65,5 @@ install(PROGRAMS scripts/gen_calibration.py if (CATKIN_ENABLE_TESTING) - - # these dependencies are only needed for unit testing - find_package(roslaunch REQUIRED) - find_package(rostest REQUIRED) - - # Download packet capture (PCAP) files containing test data. - # Store them in devel-space, so rostest can easily find them. - catkin_download_test_data( - ${PROJECT_NAME}_tests_class.pcap - http://download.ros.org/data/velodyne/class.pcap - DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests - MD5 65808d25772101358a3719b451b3d015) - catkin_download_test_data( - ${PROJECT_NAME}_tests_32e.pcap - http://download.ros.org/data/velodyne/32e.pcap - DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests - MD5 e41d02aac34f0967c03a5597e1d554a9) - catkin_download_test_data( - ${PROJECT_NAME}_tests_64e_s2.1-300-sztaki.pcap - http://download.ros.org/data/velodyne/64e_s2.1-300-sztaki.pcap - DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests - MD5 176c900ffb698f9b948a13e281ffc1a2) - catkin_download_test_data( - ${PROJECT_NAME}_tests_vlp16.pcap - http://download.ros.org/data/velodyne/vlp16.pcap - DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests - MD5 f45c2bb1d7ee358274e423ea3b66fd73) - - # unit tests - add_rostest(tests/cloud_node_hz.test) - add_rostest(tests/cloud_nodelet_hz.test) - add_rostest(tests/cloud_node_32e_hz.test) - add_rostest(tests/cloud_nodelet_32e_hz.test) - add_rostest(tests/cloud_node_64e_s2.1_hz.test) - add_rostest(tests/cloud_nodelet_64e_s2.1_hz.test) - add_rostest(tests/cloud_node_vlp16_hz.test) - add_rostest(tests/cloud_nodelet_vlp16_hz.test) - - ## These tests don't work well enough to be worth the effort of - ## running them: - #add_rostest(tests/transform_node_hz.test) - #add_rostest(tests/transform_nodelet_hz.test) - - # parse check all the launch/*.launch files - roslaunch_add_file_check(launch) - -endif (CATKIN_ENABLE_TESTING) + add_subdirectory(tests) +endif() diff --git a/velodyne_pointcloud/tests/CMakeLists.txt b/velodyne_pointcloud/tests/CMakeLists.txt new file mode 100644 index 000000000..aae7b1293 --- /dev/null +++ b/velodyne_pointcloud/tests/CMakeLists.txt @@ -0,0 +1,53 @@ +### Unit tests +# +# Only configured when CATKIN_ENABLE_TESTING is true. + +# these dependencies are only needed for unit testing +find_package(roslaunch REQUIRED) +find_package(rostest REQUIRED) + +# C++ gtests +catkin_add_gtest(test_calibration test_calibration.cpp) +add_dependencies(test_calibration ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_calibration velodyne_rawdata ${catkin_LIBRARIES}) + +# Download packet capture (PCAP) files containing test data. +# Store them in devel-space, so rostest can easily find them. +catkin_download_test_data( + ${PROJECT_NAME}_tests_class.pcap + http://download.ros.org/data/velodyne/class.pcap + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests + MD5 65808d25772101358a3719b451b3d015) +catkin_download_test_data( + ${PROJECT_NAME}_tests_32e.pcap + http://download.ros.org/data/velodyne/32e.pcap + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests + MD5 e41d02aac34f0967c03a5597e1d554a9) +catkin_download_test_data( + ${PROJECT_NAME}_tests_64e_s2.1-300-sztaki.pcap + http://download.ros.org/data/velodyne/64e_s2.1-300-sztaki.pcap + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests + MD5 176c900ffb698f9b948a13e281ffc1a2) +catkin_download_test_data( + ${PROJECT_NAME}_tests_vlp16.pcap + http://download.ros.org/data/velodyne/vlp16.pcap + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests + MD5 f45c2bb1d7ee358274e423ea3b66fd73) + +# run rostests +add_rostest(cloud_node_hz.test) +add_rostest(cloud_nodelet_hz.test) +add_rostest(cloud_node_32e_hz.test) +add_rostest(cloud_nodelet_32e_hz.test) +add_rostest(cloud_node_64e_s2.1_hz.test) +add_rostest(cloud_nodelet_64e_s2.1_hz.test) +add_rostest(cloud_node_vlp16_hz.test) +add_rostest(cloud_nodelet_vlp16_hz.test) + +## These tests don't work well enough to be worth the effort of +## running them: +#add_rostest(tests/transform_node_hz.test) +#add_rostest(tests/transform_nodelet_hz.test) + +# parse check all the launch/*.launch files +roslaunch_add_file_check(../launch) diff --git a/velodyne_pointcloud/tests/issue_84_float_intensities.yaml b/velodyne_pointcloud/tests/issue_84_float_intensities.yaml new file mode 100644 index 000000000..6c5f572b1 --- /dev/null +++ b/velodyne_pointcloud/tests/issue_84_float_intensities.yaml @@ -0,0 +1,239 @@ +# test data from an HDL-64E S2 containing float min_intensity values +# causes issue #84 failure +lasers: +- {dist_correction: 1.1897507, dist_correction_x: 1.2184412, dist_correction_y: 1.1993269, + focal_distance: 22.5, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 0, + min_intensity: 40.0, rot_correction: -0.08193448173487379, vert_correction: -0.12118950050089745, + vert_offset_correction: 0.21533014000000003} +- {dist_correction: 1.4128801999999998, dist_correction_x: 1.4313695000000002, dist_correction_y: 1.4312958, + focal_distance: 24.0, focal_slope: 0.80000001, horiz_offset_correction: -0.025999999, + laser_id: 1, min_intensity: 40.0, rot_correction: -0.04631400641637322, vert_correction: -0.1154246814722187, + vert_offset_correction: 0.21490976} +- {dist_correction: 1.352054, dist_correction_x: 1.3625516, dist_correction_y: 1.3604144, + focal_distance: 22.0, focal_slope: 1.15, horiz_offset_correction: 0.025999999, laser_id: 2, + min_intensity: 40.0, rot_correction: 0.05367805144003262, vert_correction: 0.007649359057937743, + vert_offset_correction: 0.20602308000000003} +- {dist_correction: 1.3877103, dist_correction_x: 1.4052618000000001, dist_correction_y: 1.4102048999999999, + focal_distance: 21.0, focal_slope: 1.2, horiz_offset_correction: -0.025999999, laser_id: 3, + min_intensity: 20.0, rot_correction: 0.09201863985105889, vert_correction: 0.01508908679395137, + vert_offset_correction: 0.20548805000000003} +- {dist_correction: 1.2740263, dist_correction_x: 1.2988675, dist_correction_y: 1.2487833000000002, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: 0.025999999, laser_id: 4, + min_intensity: 10.0, rot_correction: -0.007693320585669901, vert_correction: -0.1102984033676868, + vert_offset_correction: 0.21453644000000002} +- {dist_correction: 1.2418116000000001, dist_correction_x: 1.252481, dist_correction_y: 1.2255676, + focal_distance: 24.0, focal_slope: 1.38, horiz_offset_correction: -0.025999999, + laser_id: 5, min_intensity: 28.0, rot_correction: 0.032006458688310216, vert_correction: -0.10399361088436754, + vert_offset_correction: 0.21407784} +- {dist_correction: 1.3176793, dist_correction_x: 1.3353647000000002, dist_correction_y: 1.3361707999999999, + focal_distance: 24.0, focal_slope: 1.2, horiz_offset_correction: 0.025999999, laser_id: 6, + min_intensity: 40.0, rot_correction: -0.021861479338351635, vert_correction: -0.14544533071754853, + vert_offset_correction: 0.21710571} +- {dist_correction: 1.3708466000000001, dist_correction_x: 1.3905527000000002, dist_correction_y: 1.3970328, + focal_distance: 24.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999, + laser_id: 7, min_intensity: 40.0, rot_correction: 0.01761759781572604, vert_correction: -0.139035509555832, + vert_offset_correction: 0.21663536} +- {dist_correction: 1.2544352, dist_correction_x: 1.2857481000000002, dist_correction_y: 1.2282503, + focal_distance: 24.0, focal_slope: 1.22, horiz_offset_correction: 0.025999999, laser_id: 8, + min_intensity: 40.0, rot_correction: 0.06658719948349573, vert_correction: -0.09889517252673422, + vert_offset_correction: 0.21370745} +- {dist_correction: 1.3738158999999999, dist_correction_x: 1.3938158000000003, dist_correction_y: 1.3844524000000002, + focal_distance: 24.0, focal_slope: 1.15, horiz_offset_correction: -0.025999999, + laser_id: 9, min_intensity: 40.0, rot_correction: 0.10588806804896926, vert_correction: -0.09225143361201914, + vert_offset_correction: 0.21322533} +- {dist_correction: 1.3156921, dist_correction_x: 1.3354019, dist_correction_y: 1.2833449, + focal_distance: 24.0, focal_slope: 1.3200001, horiz_offset_correction: 0.025999999, + laser_id: 10, min_intensity: 40.0, rot_correction: 0.052778260906800954, vert_correction: -0.134421316289823, + vert_offset_correction: 0.2162973} +- {dist_correction: 1.3696194, dist_correction_x: 1.3846521, dist_correction_y: 1.3639107000000001, + focal_distance: 20.0, focal_slope: 1.25, horiz_offset_correction: -0.025999999, + laser_id: 11, min_intensity: 40.0, rot_correction: 0.09284370411236641, vert_correction: -0.12714737563412706, + vert_offset_correction: 0.21576523000000003} +- {dist_correction: 1.2275657, dist_correction_x: 1.2543072, dist_correction_y: 1.2294422, + focal_distance: 24.0, focal_slope: 1.0700001, horiz_offset_correction: 0.025999999, + laser_id: 12, min_intensity: 40.0, rot_correction: -0.08319450648038783, vert_correction: -0.0502394945048745, + vert_offset_correction: 0.21018864} +- {dist_correction: 1.3757889, dist_correction_x: 1.3849341999999998, dist_correction_y: 1.3686421000000002, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: -0.025999999, laser_id: 13, + min_intensity: 40.0, rot_correction: -0.04344974309026683, vert_correction: -0.04379427282993976, + vert_offset_correction: 0.20972416} +- {dist_correction: 1.3319664, dist_correction_x: 1.3705452, dist_correction_y: 1.3209669, + focal_distance: 24.0, focal_slope: 1.05, horiz_offset_correction: 0.025999999, laser_id: 14, + min_intensity: 40.0, rot_correction: -0.09661677957860802, vert_correction: -0.08535601123784947, + vert_offset_correction: 0.21272558} +- {dist_correction: 1.3714989, dist_correction_x: 1.3624129999999999, dist_correction_y: 1.3403807, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: -0.025999999, laser_id: 15, + min_intensity: 40.0, rot_correction: -0.056929933194385184, vert_correction: -0.07898063589425029, + vert_offset_correction: 0.21226404} +- {dist_correction: 1.2595838000000001, dist_correction_x: 1.2785926, dist_correction_y: 1.2370594, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: 0.025999999, laser_id: 16, + min_intensity: 40.0, rot_correction: -0.008102055927068873, vert_correction: -0.03910078576535417, + vert_offset_correction: 0.20938608} +- {dist_correction: 1.3786241000000001, dist_correction_x: 1.3948055, dist_correction_y: 1.3862044, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: -0.025999999, laser_id: 17, + min_intensity: 40.0, rot_correction: 0.03149012048240021, vert_correction: -0.03191447983673011, + vert_offset_correction: 0.20886870999999999} +- {dist_correction: 1.3628464, dist_correction_x: 1.3785178, dist_correction_y: 1.3152992000000001, + focal_distance: 20.5, focal_slope: 1.2, horiz_offset_correction: 0.025999999, laser_id: 18, + min_intensity: 10.0, rot_correction: -0.021944024685324706, vert_correction: -0.07495714058693254, + vert_offset_correction: 0.21197302} +- {dist_correction: 1.394641, dist_correction_x: 1.3899173999999999, dist_correction_y: 1.370442, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: -0.025999999, laser_id: 19, + min_intensity: 10.0, rot_correction: 0.01766883719190609, vert_correction: -0.06771695808962866, + vert_offset_correction: 0.21144976000000001} +- {dist_correction: 1.2123122, dist_correction_x: 1.2413981, dist_correction_y: 1.2070784, + focal_distance: 23.5, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 20, + min_intensity: 5.0, rot_correction: 0.06651143299533739, vert_correction: -0.027462144914892576, + vert_offset_correction: 0.20854828000000003} +- {dist_correction: 1.2599731, dist_correction_x: 1.2764229, dist_correction_y: 1.2986679, + focal_distance: 21.0, focal_slope: 1.2, horiz_offset_correction: -0.025999999, laser_id: 21, + min_intensity: 5.0, rot_correction: 0.10511566735251142, vert_correction: -0.020434314882349612, + vert_offset_correction: 0.20804264} +- {dist_correction: 1.3053941, dist_correction_x: 1.3411044, dist_correction_y: 1.2835222, + focal_distance: 24.0, focal_slope: 1.42, horiz_offset_correction: 0.025999999, laser_id: 22, + min_intensity: 30.0, rot_correction: 0.052657269447064954, vert_correction: -0.06262826041455072, + vert_offset_correction: 0.21108229} +- {dist_correction: 1.2550185, dist_correction_x: 1.2702521, dist_correction_y: 1.2836005000000001, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: -0.025999999, laser_id: 23, + min_intensity: 40.0, rot_correction: 0.09175727679557273, vert_correction: -0.05606926072567847, + vert_offset_correction: 0.21060900000000002} +- {dist_correction: 1.2748341, dist_correction_x: 1.3196121, dist_correction_y: 1.2784751, + focal_distance: 20.5, focal_slope: 1.25, horiz_offset_correction: 0.025999999, laser_id: 24, + min_intensity: 10.0, rot_correction: -0.08344753035270795, vert_correction: 0.020524792750772996, + vert_offset_correction: 0.20509708000000001} +- {dist_correction: 1.3777713, dist_correction_x: 1.3645123000000001, dist_correction_y: 1.3802203, + focal_distance: 24.0, focal_slope: 1.3, horiz_offset_correction: -0.025999999, laser_id: 25, + max_intensity: 230.0, rot_correction: -0.04351660490858147, vert_correction: 0.02702150163864157, + vert_offset_correction: 0.20462966999999999} +- {dist_correction: 1.3718602000000002, dist_correction_x: 1.4046102999999999, dist_correction_y: 1.3834868, + focal_distance: 19.0, focal_slope: 1.35, horiz_offset_correction: 0.025999999, laser_id: 26, + max_intensity: 245.0, rot_correction: -0.09689230074498635, vert_correction: -0.014916840599137901, + vert_offset_correction: 0.2076458} +- {dist_correction: 1.3833778, dist_correction_x: 1.375231, dist_correction_y: 1.3606032, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: -0.025999999, laser_id: 27, + max_intensity: 240.0, rot_correction: -0.057451870416535586, vert_correction: -0.00817206789015045, + vert_offset_correction: 0.20716073999999998} +- {dist_correction: 1.2951183000000002, dist_correction_x: 1.3258408, dist_correction_y: 1.2997318999999998, + focal_distance: 24.0, focal_slope: 1.4, horiz_offset_correction: 0.025999999, laser_id: 28, + max_intensity: 240.0, rot_correction: -0.008271954654038412, vert_correction: 0.032168250709970085, + vert_offset_correction: 0.20425926} +- {dist_correction: 1.4026823, dist_correction_x: 1.4151775000000002, dist_correction_y: 1.4065117, + focal_distance: 24.0, focal_slope: 1.05, horiz_offset_correction: -0.025999999, + laser_id: 29, rot_correction: 0.03059445241686176, vert_correction: 0.03821143404637868, + vert_offset_correction: 0.2038242} +- {dist_correction: 1.3756502000000002, dist_correction_x: 1.3986545000000001, dist_correction_y: 1.3567966000000002, + focal_distance: 24.0, focal_slope: 1.37, horiz_offset_correction: 0.025999999, laser_id: 30, + rot_correction: -0.022168507188386963, vert_correction: -0.003020972948984465, vert_offset_correction: 0.20679033000000002} +- {dist_correction: 1.4303774999999999, dist_correction_x: 1.4219699000000001, dist_correction_y: 1.4110721000000002, + focal_distance: 24.0, focal_slope: 1.38, horiz_offset_correction: -0.025999999, + laser_id: 31, rot_correction: 0.017020332183059567, vert_correction: 0.0032341127317709376, + vert_offset_correction: 0.20634056} +- {dist_correction: 1.2299187999999999, dist_correction_x: 1.221242, dist_correction_y: 1.159202, + focal_distance: 12.8, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 32, + rot_correction: -0.13428924897065386, vert_correction: -0.3917846862503118, vert_offset_correction: 0.15982219} +- {dist_correction: 1.3144859, dist_correction_x: 1.3336081, dist_correction_y: 1.3461792, + focal_distance: 0.25, focal_slope: 1.02, horiz_offset_correction: -0.025999999, + laser_id: 33, rot_correction: -0.07421947892390637, vert_correction: -0.3836205247016729, + vert_offset_correction: 0.15923027} +- {dist_correction: 1.2725937999999999, dist_correction_x: 1.2842772, dist_correction_y: 1.3025389, + focal_distance: 0.25, focal_slope: 0.94999999, horiz_offset_correction: 0.025999999, + laser_id: 34, rot_correction: 0.08138446391038806, vert_correction: -0.1992609420279063, + vert_offset_correction: 0.14669841} +- {dist_correction: 1.3357777, dist_correction_x: 1.3343283, dist_correction_y: 1.3631558000000001, + focal_distance: 9.0, focal_slope: 1.5, horiz_offset_correction: -0.025999999, laser_id: 35, + rot_correction: 0.1383208914749199, vert_correction: -0.18701489169116567, vert_offset_correction: 0.14590834} +- {dist_correction: 1.1742934, dist_correction_x: 1.2244499, dist_correction_y: 1.1818057, + focal_distance: 10.8, focal_slope: 1.38, horiz_offset_correction: 0.025999999, laser_id: 36, + rot_correction: -0.013445351586919306, vert_correction: -0.37738235143590226, vert_offset_correction: 0.15878062} +- {dist_correction: 1.090762, dist_correction_x: 1.1042352, dist_correction_y: 1.0975322, + focal_distance: 12.5, focal_slope: 1.55, horiz_offset_correction: -0.025999999, + laser_id: 37, rot_correction: 0.04734804593033903, vert_correction: -0.3673083633532685, + vert_offset_correction: 0.15805914} +- {dist_correction: 1.2801097, dist_correction_x: 1.2734876, dist_correction_y: 1.2697346, + focal_distance: 6.0, focal_slope: 1.1, horiz_offset_correction: 0.025999999, laser_id: 38, + rot_correction: -0.03680020095578886, vert_correction: -0.4294796616630621, vert_offset_correction: 0.16260902000000002} +- {dist_correction: 1.2865338, dist_correction_x: 1.2686511, dist_correction_y: 1.29416, + focal_distance: 0.25, focal_slope: 1.0, horiz_offset_correction: -0.025999999, laser_id: 39, + rot_correction: 0.025992554995433074, vert_correction: -0.42099842614921335, vert_offset_correction: 0.16197392000000002} +- {dist_correction: 1.2932777, dist_correction_x: 1.3400191000000001, dist_correction_y: 1.3263618, + focal_distance: 0.25, focal_slope: 1.02, horiz_offset_correction: 0.025999999, laser_id: 40, + rot_correction: 0.10601307725664336, vert_correction: -0.3565455112681652, vert_offset_correction: 0.15729448} +- {dist_correction: 1.2614787, dist_correction_x: 1.2626826, dist_correction_y: 1.267609, + focal_distance: 8.5, focal_slope: 1.65, horiz_offset_correction: -0.025999999, laser_id: 41, + rot_correction: 0.16733068965072595, vert_correction: -0.3479014190818128, vert_offset_correction: 0.15668478} +- {dist_correction: 1.2428966, dist_correction_x: 1.2460679000000001, dist_correction_y: 1.2397518, + focal_distance: 0.25, focal_slope: 1.2, horiz_offset_correction: 0.025999999, laser_id: 42, + rot_correction: 0.08594727468321833, vert_correction: -0.4110102035460227, vert_offset_correction: 0.16123213} +- {dist_correction: 1.3061783, dist_correction_x: 1.2655383, dist_correction_y: 1.2857359, + focal_distance: 10.0, focal_slope: 1.6799999, horiz_offset_correction: -0.025999999, + laser_id: 43, rot_correction: 0.1464050331680134, vert_correction: -0.4015579191962419, + vert_offset_correction: 0.16053604} +- {dist_correction: 1.2317036000000001, dist_correction_x: 1.2393882, dist_correction_y: 1.2123593, + focal_distance: 12.5, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 44, + rot_correction: -0.13261467013054762, vert_correction: -0.2838808920696538, vert_offset_correction: 0.1522797} +- {dist_correction: 1.2567677000000002, dist_correction_x: 1.2904861, dist_correction_y: 1.2915567, + focal_distance: 0.25, focal_slope: 0.47999999, horiz_offset_correction: -0.025999999, + laser_id: 45, rot_correction: -0.07009130400404175, vert_correction: -0.276291415083819, + vert_offset_correction: 0.15176907} +- {dist_correction: 1.2793438000000001, dist_correction_x: 1.2915629999999998, dist_correction_y: 1.2718159, + focal_distance: 12.0, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 46, + rot_correction: -0.15533259824081613, vert_correction: -0.33574467724289675, vert_offset_correction: 0.15583374} +- {dist_correction: 1.2330788, dist_correction_x: 1.2624972, dist_correction_y: 1.2636649, + focal_distance: 2.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999, + laser_id: 47, rot_correction: -0.09347288483573485, vert_correction: -0.3300846093852642, + vert_offset_correction: 0.15543998} +- {dist_correction: 1.1445625, dist_correction_x: 1.2045525000000001, dist_correction_y: 1.1558383, + focal_distance: 23.2, focal_slope: 0.75, horiz_offset_correction: 0.025999999, laser_id: 48, + rot_correction: -0.014086204897792162, vert_correction: -0.2692770589931014, vert_offset_correction: 0.15129910000000002} +- {dist_correction: 1.2741866, dist_correction_x: 1.2946111, dist_correction_y: 1.2808211, + focal_distance: 18.0, focal_slope: 0.92000002, horiz_offset_correction: -0.025999999, + laser_id: 49, rot_correction: 0.04602459591980752, vert_correction: -0.25952585210233153, + vert_offset_correction: 0.15064875} +- {dist_correction: 1.2584636, dist_correction_x: 1.2985907, dist_correction_y: 1.2481342, + focal_distance: 15.8, focal_slope: 1.28, horiz_offset_correction: 0.025999999, laser_id: 50, + rot_correction: -0.035928232716163244, vert_correction: -0.3234839312338543, vert_offset_correction: 0.1549827} +- {dist_correction: 1.1207238, dist_correction_x: 1.1458776000000002, dist_correction_y: 1.1290197000000002, + focal_distance: 18.0, focal_slope: 0.89999998, horiz_offset_correction: -0.025999999, + laser_id: 51, rot_correction: 0.025398581053735863, vert_correction: -0.3141198558244693, + vert_offset_correction: 0.15433743} +- {dist_correction: 1.3357715, dist_correction_x: 1.3748308, dist_correction_y: 1.3750087, + focal_distance: 2.2, focal_slope: 0.64999998, horiz_offset_correction: 0.025999999, + laser_id: 52, rot_correction: 0.10206324357040601, vert_correction: -0.2503763474711867, + vert_offset_correction: 0.15004159} +- {dist_correction: 1.2831325000000002, dist_correction_x: 1.2906917000000002, dist_correction_y: 1.3047183000000002, + focal_distance: 8.2, focal_slope: 1.65, horiz_offset_correction: -0.025999999, laser_id: 53, + rot_correction: 0.16118048446991431, vert_correction: -0.2388694789792507, vert_offset_correction: 0.14928201} +- {dist_correction: 1.4319325, dist_correction_x: 1.470513, dist_correction_y: 1.4623492, + focal_distance: 6.0, focal_slope: 0.51999998, horiz_offset_correction: 0.025999999, + laser_id: 54, rot_correction: 0.08241589764778065, vert_correction: -0.30499605989320633, + vert_offset_correction: 0.15371249} +- {dist_correction: 1.2852434, dist_correction_x: 1.2985229, dist_correction_y: 1.271758, + focal_distance: 12.5, focal_slope: 2.0, horiz_offset_correction: -0.025999999, laser_id: 55, + rot_correction: 0.142266131454566, vert_correction: -0.2947717466020984, vert_offset_correction: 0.15301642} +- {dist_correction: 1.4007397000000001, dist_correction_x: 1.4001398, dist_correction_y: 1.3912132, + focal_distance: 8.5, focal_slope: 1.7, horiz_offset_correction: 0.025999999, laser_id: 56, + rot_correction: -0.12786652808043583, vert_correction: -0.17641617036891985, vert_offset_correction: 0.14522751} +- {dist_correction: 1.2880724000000001, dist_correction_x: 1.3014014, dist_correction_y: 1.3205367000000001, + focal_distance: 0.25, focal_slope: 1.2, horiz_offset_correction: -0.025999999, laser_id: 57, + rot_correction: -0.07017803115990259, vert_correction: -0.17070459784506095, vert_offset_correction: 0.14486169000000002} +- {dist_correction: 1.4760434, dist_correction_x: 1.4308182, dist_correction_y: 1.4627808, + focal_distance: 11.5, focal_slope: 2.0, horiz_offset_correction: 0.025999999, laser_id: 58, + rot_correction: -0.15113763829712393, vert_correction: -0.2281125435603116, vert_offset_correction: 0.14857577} +- {dist_correction: 1.303936, dist_correction_x: 1.3230009, dist_correction_y: 1.3335196, + focal_distance: 0.25, focal_slope: 0.64999998, horiz_offset_correction: -0.025999999, + laser_id: 59, rot_correction: -0.0918767934520908, vert_correction: -0.2232191988430801, + vert_offset_correction: 0.14825568} +- {dist_correction: 1.3472046, dist_correction_x: 1.3541069, dist_correction_y: 1.3651146, + focal_distance: 3.5, focal_slope: 1.38, horiz_offset_correction: 0.025999999, laser_id: 60, + rot_correction: -0.013572408240338915, vert_correction: -0.16291245497181386, vert_offset_correction: 0.14436376} +- {dist_correction: 1.3001047000000001, dist_correction_x: 1.3096320000000001, dist_correction_y: 1.3519560000000002, + focal_distance: 3.5, focal_slope: 1.35, horiz_offset_correction: -0.025999999, laser_id: 61, + rot_correction: 0.04349449333228796, vert_correction: -0.1540625535600664, vert_offset_correction: 0.14379979} +- {dist_correction: 1.2880972, dist_correction_x: 1.2842999000000002, dist_correction_y: 1.2916876, + focal_distance: 12.0, focal_slope: 1.35, horiz_offset_correction: 0.025999999, laser_id: 62, + rot_correction: -0.03466167345792974, vert_correction: -0.21484439350701803, vert_offset_correction: 0.14770949} +- {dist_correction: 1.3186316, dist_correction_x: 1.3250179, dist_correction_y: 1.3431560999999999, + focal_distance: 10.5, focal_slope: 0.60000002, horiz_offset_correction: -0.025999999, + laser_id: 63, rot_correction: 0.02351688573662471, vert_correction: -0.20683046990039078, + vert_offset_correction: 0.14718871} +num_lasers: 64 diff --git a/velodyne_pointcloud/tests/test_calibration.cpp b/velodyne_pointcloud/tests/test_calibration.cpp new file mode 100644 index 000000000..896b17412 --- /dev/null +++ b/velodyne_pointcloud/tests/test_calibration.cpp @@ -0,0 +1,69 @@ +// +// C++ unit tests for calibration interface. +// + +#include + +#include +#include +using namespace velodyne_pointcloud; + +// global test data +std::string g_package_name("velodyne_pointcloud"); +std::string g_package_path; + +void init_global_data(void) +{ + g_package_path = ros::package::getPath(g_package_name); +} + +/////////////////////////////////////////////////////////////// +// Test cases +/////////////////////////////////////////////////////////////// + +TEST(Calibration, missing_file) +{ + Calibration calibration; + calibration.read("./no_such_file.yaml"); + EXPECT_FALSE(calibration.initialized); +} + +TEST(Calibration, vlp16) +{ + Calibration calibration(g_package_path + "/params/VLP16db.yaml"); + EXPECT_TRUE(calibration.initialized); +} + +TEST(Calibration, hdl32e) +{ + Calibration calibration(g_package_path + "/params/32db.yaml"); + EXPECT_TRUE(calibration.initialized); +} + +TEST(Calibration, hdl64e) +{ + Calibration calibration(g_package_path + "/params/64e_utexas.yaml"); + EXPECT_TRUE(calibration.initialized); +} + +TEST(Calibration, hdl64e_s21) +{ + Calibration calibration(g_package_path + "/params/64e_s2.1-sztaki.yaml"); + EXPECT_TRUE(calibration.initialized); +} + +TEST(Calibration, hdl64e_s2_float_intensities) +{ + Calibration calibration(g_package_path + + "/tests/issue_84_float_intensities.yaml"); + EXPECT_TRUE(calibration.initialized); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + init_global_data(); + return RUN_ALL_TESTS(); +} + From 9a5d583c1787eb700847ecfc80a17b04986694ee Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Mon, 21 Mar 2016 15:38:34 -0500 Subject: [PATCH 30/66] calibration: read all intensities as float, then convert (#84) --- velodyne_pointcloud/src/lib/calibration.cc | 22 ++++++---------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/velodyne_pointcloud/src/lib/calibration.cc b/velodyne_pointcloud/src/lib/calibration.cc index e87e0c437..c8a37d9a2 100644 --- a/velodyne_pointcloud/src/lib/calibration.cc +++ b/velodyne_pointcloud/src/lib/calibration.cc @@ -96,14 +96,9 @@ namespace velodyne_pointcloud max_intensity_node = pName; #endif if (max_intensity_node) { - try { - *max_intensity_node >> correction.second.max_intensity; - } catch(const YAML::InvalidScalar & exc) { - float max_intensity_float; - *max_intensity_node >> max_intensity_float; - correction.second.max_intensity = floor(max_intensity_float); - ROS_WARN_ONCE("Implicitly converting 'max_intensity' values from floats to ints."); - } + float max_intensity_float; + *max_intensity_node >> max_intensity_float; + correction.second.max_intensity = floor(max_intensity_float); } else { correction.second.max_intensity = 255; @@ -120,14 +115,9 @@ namespace velodyne_pointcloud min_intensity_node = pName; #endif if (min_intensity_node) { - try { - *min_intensity_node >> correction.second.min_intensity; - } catch(const YAML::InvalidScalar & exc) { - float min_intensity_float; - *min_intensity_node >> min_intensity_float; - correction.second.min_intensity = floor(min_intensity_float); - ROS_WARN_ONCE("Implicitly converting 'min_intensity' values from floats to ints."); - } + float min_intensity_float; + *min_intensity_node >> min_intensity_float; + correction.second.min_intensity = floor(min_intensity_float); } else { correction.second.min_intensity = 0; From 8e581999579af2726790205ffadf2d7259cd09f5 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Tue, 22 Mar 2016 11:49:23 -0500 Subject: [PATCH 31/66] calibration: unit test case improvements (#84) --- .../include/velodyne_pointcloud/calibration.h | 28 +++-- velodyne_pointcloud/src/lib/calibration.cc | 6 +- .../tests/test_calibration.cpp | 107 +++++++++++++++++- 3 files changed, 123 insertions(+), 18 deletions(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/calibration.h b/velodyne_pointcloud/include/velodyne_pointcloud/calibration.h index 32b2149c9..483709fd3 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/calibration.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/calibration.h @@ -19,10 +19,13 @@ namespace velodyne_pointcloud { /** \brief correction values for a single laser * - * Correction values for a single laser (as provided by db.xml from veleodyne) - * Includes parameters for Velodyne HDL-64E S2.1 calibration. + * Correction values for a single laser (as provided by db.xml from + * Velodyne). Includes parameters for Velodyne HDL-64E S2.1. + * * http://velodynelidar.com/lidar/products/manual/63-HDL64E%20S2%20Manual_Rev%20D_2011_web.pdf - **/ + */ + + /** \brief Correction information for a single laser. */ struct LaserCorrection { /** parameters in db.xml */ @@ -40,15 +43,15 @@ namespace velodyne_pointcloud { float focal_slope; /** cached values calculated when the calibration file is read */ - float cos_rot_correction; ///< cached cosine of rot_correction - float sin_rot_correction; ///< cached sine of rot_correction - float cos_vert_correction; ///< cached cosine of vert_correction - float sin_vert_correction; ///< cached sine of vert_correction + float cos_rot_correction; ///< cosine of rot_correction + float sin_rot_correction; ///< sine of rot_correction + float cos_vert_correction; ///< cosine of vert_correction + float sin_vert_correction; ///< sine of vert_correction int laser_ring; ///< ring number for this laser }; - /** \brief Calibration class storing entire configuration for the Velodyne */ + /** \brief Calibration information for the entire device. */ class Calibration { public: @@ -56,11 +59,16 @@ namespace velodyne_pointcloud { std::map laser_corrections; int num_lasers; bool initialized; + bool ros_info; public: - Calibration() : initialized(false) {} - Calibration(const std::string& calibration_file) { + Calibration(bool info=true): + initialized(false), ros_info(info) {} + Calibration(const std::string& calibration_file, + bool info=true): + ros_info(info) + { read(calibration_file); } diff --git a/velodyne_pointcloud/src/lib/calibration.cc b/velodyne_pointcloud/src/lib/calibration.cc index c8a37d9a2..c470c5918 100644 --- a/velodyne_pointcloud/src/lib/calibration.cc +++ b/velodyne_pointcloud/src/lib/calibration.cc @@ -176,8 +176,10 @@ namespace velodyne_pointcloud // store this ring number with its corresponding laser number calibration.laser_corrections[next_index].laser_ring = ring; next_angle = min_seen; - ROS_INFO("laser_ring[%2u] = %2u, angle = %+.6f", - next_index, ring, next_angle); + if (calibration.ros_info) { + ROS_INFO("laser_ring[%2u] = %2u, angle = %+.6f", + next_index, ring, next_angle); + } } } } diff --git a/velodyne_pointcloud/tests/test_calibration.cpp b/velodyne_pointcloud/tests/test_calibration.cpp index 896b17412..5139eca04 100644 --- a/velodyne_pointcloud/tests/test_calibration.cpp +++ b/velodyne_pointcloud/tests/test_calibration.cpp @@ -23,40 +23,135 @@ void init_global_data(void) TEST(Calibration, missing_file) { - Calibration calibration; + Calibration calibration(false); calibration.read("./no_such_file.yaml"); EXPECT_FALSE(calibration.initialized); } TEST(Calibration, vlp16) { - Calibration calibration(g_package_path + "/params/VLP16db.yaml"); + Calibration calibration(g_package_path + "/params/VLP16db.yaml", false); EXPECT_TRUE(calibration.initialized); + ASSERT_EQ(calibration.num_lasers, 16); + + // check some values for the first laser: + LaserCorrection laser = calibration.laser_corrections[0]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.2617993877991494); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 0); + + // check similar values for the last laser: + laser = calibration.laser_corrections[15]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, 0.2617993877991494); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 0); } TEST(Calibration, hdl32e) { - Calibration calibration(g_package_path + "/params/32db.yaml"); + Calibration calibration(g_package_path + "/params/32db.yaml", false); EXPECT_TRUE(calibration.initialized); + ASSERT_EQ(calibration.num_lasers, 32); + + // check some values for the first laser: + LaserCorrection laser = calibration.laser_corrections[0]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.5352924815866609); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 0); + + // check similar values for the last laser: + laser = calibration.laser_corrections[31]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, 0.18622663118779495); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 0); } TEST(Calibration, hdl64e) { - Calibration calibration(g_package_path + "/params/64e_utexas.yaml"); + Calibration calibration(g_package_path + "/params/64e_utexas.yaml", false); EXPECT_TRUE(calibration.initialized); + ASSERT_EQ(calibration.num_lasers, 64); + + // check some values for the first laser: + LaserCorrection laser = calibration.laser_corrections[0]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.124932751059532); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 0); + + // check similar values for the last laser: + laser = calibration.laser_corrections[63]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.209881335496902); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 0); } TEST(Calibration, hdl64e_s21) { - Calibration calibration(g_package_path + "/params/64e_s2.1-sztaki.yaml"); + Calibration calibration(g_package_path + "/params/64e_s2.1-sztaki.yaml", + false); EXPECT_TRUE(calibration.initialized); + ASSERT_EQ(calibration.num_lasers, 64); + + // check some values for the first laser: + LaserCorrection laser = calibration.laser_corrections[0]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.15304134919741974); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.025999999); + EXPECT_EQ(laser.max_intensity, 235); + EXPECT_EQ(laser.min_intensity, 30); + + // check similar values for the last laser: + laser = calibration.laser_corrections[63]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.2106649408137298); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, -0.025999999); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 0); } TEST(Calibration, hdl64e_s2_float_intensities) { Calibration calibration(g_package_path + - "/tests/issue_84_float_intensities.yaml"); + "/tests/issue_84_float_intensities.yaml", + false); EXPECT_TRUE(calibration.initialized); + ASSERT_EQ(calibration.num_lasers, 64); + + // check some values for the first laser: + LaserCorrection laser = calibration.laser_corrections[0]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.12118950050089745); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.025999999); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 40); + + // check similar values for laser 26: + laser = calibration.laser_corrections[26]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.014916840599137901); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.025999999); + EXPECT_EQ(laser.max_intensity, 245); + EXPECT_EQ(laser.min_intensity, 0); + + // check similar values for the last laser: + laser = calibration.laser_corrections[63]; + EXPECT_FALSE(laser.two_pt_correction_available); + EXPECT_FLOAT_EQ(laser.vert_correction, -0.20683046990039078); + EXPECT_FLOAT_EQ(laser.horiz_offset_correction, -0.025999999); + EXPECT_EQ(laser.max_intensity, 255); + EXPECT_EQ(laser.min_intensity, 0); } // Run all the tests that were declared with TEST() From 14d0241f828700d5c55bd21e5e38afa5c9949b67 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Fri, 25 Mar 2016 11:28:33 -0500 Subject: [PATCH 32/66] prepare change history for coming Indigo release (#59) --- README.rst | 2 +- velodyne/CHANGELOG.rst | 3 +++ velodyne_driver/CHANGELOG.rst | 6 ++++++ velodyne_driver/package.xml | 1 + velodyne_msgs/CHANGELOG.rst | 3 +++ velodyne_pointcloud/CHANGELOG.rst | 23 ++++++++++++++++++++++- 6 files changed, 36 insertions(+), 2 deletions(-) diff --git a/README.rst b/README.rst index 88e71fe05..9686124b9 100644 --- a/README.rst +++ b/README.rst @@ -9,7 +9,7 @@ definition 3D LIDARs`_. The master branch normally contains code being tested for the next ROS release. It will not always work with every previous release. -The current ``master`` branch works with ROS Indigo and Hydro. It may +The current ``master`` branch works with ROS Jade, Indigo and Hydro. It may work with Groovy, but that is not guaranteed. To build for Fuerte from source, check out the ``rosbuild`` branch instead of ``master``. diff --git a/velodyne/CHANGELOG.rst b/velodyne/CHANGELOG.rst index 6eea4b978..568117341 100644 --- a/velodyne/CHANGELOG.rst +++ b/velodyne/CHANGELOG.rst @@ -1,6 +1,9 @@ Change history ============== +1.3.0 (forthcoming) +------------------- + 1.2.0 (2014-08-06) ------------------ diff --git a/velodyne_driver/CHANGELOG.rst b/velodyne_driver/CHANGELOG.rst index 628965d64..82a41ba50 100644 --- a/velodyne_driver/CHANGELOG.rst +++ b/velodyne_driver/CHANGELOG.rst @@ -1,6 +1,12 @@ Change history ============== +1.3.0 (forthcoming) +------------------- +* Add VLP-16 support. +* Add support for multiple devices. +* Contributors: Brice Rebsamen, ddillenberger, Patrick Hussey, Jack O'Quin + 1.2.0 (2014-08-06) ------------------ * Fixed bug in diagnostic rate for driver (`#16 diff --git a/velodyne_driver/package.xml b/velodyne_driver/package.xml index c8be55d26..e83160555 100644 --- a/velodyne_driver/package.xml +++ b/velodyne_driver/package.xml @@ -6,6 +6,7 @@ ROS device driver for Velodyne HDL-64E, and HDL-32 LIDARs. Jack O'Quin + Brice Rebsamen Jack O'Quin Patrick Beeson Michael Quinlan diff --git a/velodyne_msgs/CHANGELOG.rst b/velodyne_msgs/CHANGELOG.rst index b2f5ff838..b432fc9d8 100644 --- a/velodyne_msgs/CHANGELOG.rst +++ b/velodyne_msgs/CHANGELOG.rst @@ -1,6 +1,9 @@ Change history ============== +1.3.0 (forthcoming) +------------------- + 1.2.0 (2014-08-06) ------------------ diff --git a/velodyne_pointcloud/CHANGELOG.rst b/velodyne_pointcloud/CHANGELOG.rst index 2c5ea9cf0..7999224f0 100644 --- a/velodyne_pointcloud/CHANGELOG.rst +++ b/velodyne_pointcloud/CHANGELOG.rst @@ -1,8 +1,21 @@ Change history ============== +1.3.0 (forthcoming) +------------------- + +* Allow ``min_range`` as short as 0.4m. +* Add VLP-16 support (`#44`_). +* Fix several errors in point computations (`#43`_, `#47`_, `#50`_, + `#55`_, `#76`_). +* Fix several calibration file problems (`#41`_, `#42`_). +* Contributors: Bo Li, Patrick Hussey, William Woodall, Kun Li, + lemiant, Jose Luis Blanco-Claraco, Alexander Schaefer, François + Pomerleau, Jack O'Quin + 1.2.0 (2014-08-06) ------------------ + * velodyne_pointcloud: remove model-dependent "constants" from rawdata.h (`#28 `_) @@ -16,7 +29,7 @@ Change history * Contributors: Jack O'Quin, Scott K Logan, Thomas Solatges 1.1.2 (2013-11-05) -------------------- +------------------ * Move unit test data to download.ros.org (`#18`_). * Install missing gen_calibration.py script (`#20`_). @@ -108,3 +121,11 @@ Change history .. _`#17`: https://github.com/ros-drivers/velodyne/issues/17 .. _`#18`: https://github.com/ros-drivers/velodyne/issues/18 .. _`#20`: https://github.com/ros-drivers/velodyne/issues/20 +.. _`#41`: https://github.com/ros-drivers/velodyne/pull/41 +.. _`#42`: https://github.com/ros-drivers/velodyne/pull/42 +.. _`#43`: https://github.com/ros-drivers/velodyne/pull/43 +.. _`#44`: https://github.com/ros-drivers/velodyne/pull/44 +.. _`#47`: https://github.com/ros-drivers/velodyne/pull/47 +.. _`#50`: https://github.com/ros-drivers/velodyne/issue/50 +.. _`#55`: https://github.com/ros-drivers/velodyne/pull/55 +.. _`#76`: https://github.com/ros-drivers/velodyne/pull/76 From 196f2739cb40a3a9ee597dfee31b182b492d172a Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Tue, 12 Apr 2016 15:44:26 -0500 Subject: [PATCH 33/66] velodyne_pointcloud: fix transform unit tests Use tf2_ros static_transform_publisher for more consistent timing (#2) --- velodyne_pointcloud/package.xml | 1 + velodyne_pointcloud/tests/CMakeLists.txt | 5 +++-- velodyne_pointcloud/tests/static_vehicle_tf.launch | 8 ++++---- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/velodyne_pointcloud/package.xml b/velodyne_pointcloud/package.xml index 70d95f820..6ae668136 100644 --- a/velodyne_pointcloud/package.xml +++ b/velodyne_pointcloud/package.xml @@ -33,6 +33,7 @@ roslaunch rostest + tf2_ros angles nodelet diff --git a/velodyne_pointcloud/tests/CMakeLists.txt b/velodyne_pointcloud/tests/CMakeLists.txt index aae7b1293..85d173b5d 100644 --- a/velodyne_pointcloud/tests/CMakeLists.txt +++ b/velodyne_pointcloud/tests/CMakeLists.txt @@ -5,6 +5,7 @@ # these dependencies are only needed for unit testing find_package(roslaunch REQUIRED) find_package(rostest REQUIRED) +find_package(tf2_ros REQUIRED) # C++ gtests catkin_add_gtest(test_calibration test_calibration.cpp) @@ -46,8 +47,8 @@ add_rostest(cloud_nodelet_vlp16_hz.test) ## These tests don't work well enough to be worth the effort of ## running them: -#add_rostest(tests/transform_node_hz.test) -#add_rostest(tests/transform_nodelet_hz.test) +add_rostest(transform_node_hz.test) +add_rostest(transform_nodelet_hz.test) # parse check all the launch/*.launch files roslaunch_add_file_check(../launch) diff --git a/velodyne_pointcloud/tests/static_vehicle_tf.launch b/velodyne_pointcloud/tests/static_vehicle_tf.launch index 3af0c04ce..9e35585be 100644 --- a/velodyne_pointcloud/tests/static_vehicle_tf.launch +++ b/velodyne_pointcloud/tests/static_vehicle_tf.launch @@ -4,14 +4,14 @@ - + args="0.393 0.278 2.4 -0.02155 0.0164 0.00621 vehicle velodyne" /> - + args="0 0 0 0 0 0 odom vehicle" /> From 76e5f85537aedb6ab3339a964336cedf8e493dc6 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Tue, 12 Apr 2016 16:14:36 -0500 Subject: [PATCH 34/66] velodyne_pointcloud: use recommended add_dependencies() CMake variable #78 --- velodyne_pointcloud/src/conversions/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/velodyne_pointcloud/src/conversions/CMakeLists.txt b/velodyne_pointcloud/src/conversions/CMakeLists.txt index 2d149531e..8e6e25196 100644 --- a/velodyne_pointcloud/src/conversions/CMakeLists.txt +++ b/velodyne_pointcloud/src/conversions/CMakeLists.txt @@ -1,12 +1,12 @@ add_executable(cloud_node cloud_node.cc convert.cc) -add_dependencies(cloud_node ${PROJECT_NAME}_gencfg) +add_dependencies(cloud_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(cloud_node velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS cloud_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) add_library(cloud_nodelet cloud_nodelet.cc convert.cc) -add_dependencies(cloud_nodelet ${PROJECT_NAME}_gencfg) +add_dependencies(cloud_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(cloud_nodelet velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS cloud_nodelet @@ -29,14 +29,14 @@ install(TARGETS ringcolors_nodelet LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) add_executable(transform_node transform_node.cc transform.cc) -add_dependencies(transform_node ${PROJECT_NAME}_gencfg) +add_dependencies(transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(transform_node velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS transform_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) add_library(transform_nodelet transform_nodelet.cc transform.cc) -add_dependencies(transform_nodelet ${PROJECT_NAME}_gencfg) +add_dependencies(transform_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(transform_nodelet velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS transform_nodelet From 893bdcb9468aff545b68ea6c05928c784d57e99d Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Tue, 12 Apr 2016 16:24:34 -0500 Subject: [PATCH 35/66] velodyne_pointcloud: add dynamic reconfig update to change log (#78) --- velodyne_pointcloud/CHANGELOG.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/velodyne_pointcloud/CHANGELOG.rst b/velodyne_pointcloud/CHANGELOG.rst index 7999224f0..80dd6231a 100644 --- a/velodyne_pointcloud/CHANGELOG.rst +++ b/velodyne_pointcloud/CHANGELOG.rst @@ -4,6 +4,7 @@ Change history 1.3.0 (forthcoming) ------------------- +* Provide dynamic reconfiguration for TransformNodelet (`#78`_). * Allow ``min_range`` as short as 0.4m. * Add VLP-16 support (`#44`_). * Fix several errors in point computations (`#43`_, `#47`_, `#50`_, @@ -129,3 +130,4 @@ Change history .. _`#50`: https://github.com/ros-drivers/velodyne/issue/50 .. _`#55`: https://github.com/ros-drivers/velodyne/pull/55 .. _`#76`: https://github.com/ros-drivers/velodyne/pull/76 +.. _`#78`: https://github.com/ros-drivers/velodyne/pull/78 From 5e4a52eb8bc4b39364c818ec921c1fd3c3796431 Mon Sep 17 00:00:00 2001 From: Andreas Wachaja Date: Wed, 20 Apr 2016 11:31:21 +0200 Subject: [PATCH 36/66] velodyne_pointcloud: Fix compile warning "Wrong initialization order" --- velodyne_pointcloud/src/conversions/transform.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 4bf5a2e99..bce8b035e 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -25,8 +25,8 @@ namespace velodyne_pointcloud { /** @brief Constructor. */ Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh): - data_(new velodyne_rawdata::RawData()), - tf_prefix_(tf::getPrefixParam(private_nh)) + tf_prefix_(tf::getPrefixParam(private_nh)), + data_(new velodyne_rawdata::RawData()) { // Read calibration. data_->setup(private_nh); From 094db6975f41191690e354fe24b201b5a907edd6 Mon Sep 17 00:00:00 2001 From: Andreas Wachaja Date: Fri, 22 Apr 2016 13:51:11 +0200 Subject: [PATCH 37/66] velodyne_driver: Make input destructors virtual --- velodyne_driver/include/velodyne_driver/input.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/velodyne_driver/include/velodyne_driver/input.h b/velodyne_driver/include/velodyne_driver/input.h index eaab2e2a1..adea8fab0 100644 --- a/velodyne_driver/include/velodyne_driver/input.h +++ b/velodyne_driver/include/velodyne_driver/input.h @@ -48,6 +48,7 @@ namespace velodyne_driver { public: Input() {} + virtual ~Input() {} /** @brief Read one Velodyne packet. * @@ -75,7 +76,7 @@ namespace velodyne_driver public: InputSocket(ros::NodeHandle private_nh, uint16_t udp_port = UDP_PORT_NUMBER); - ~InputSocket(); + virtual ~InputSocket(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); void setDeviceIP( const std::string& ip ); @@ -100,7 +101,7 @@ namespace velodyne_driver bool read_once=false, bool read_fast=false, double repeat_delay=0.0); - ~InputPCAP(); + virtual ~InputPCAP(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); void setDeviceIP( const std::string& ip ); From ed051a80c16852dff5ec1fa581aa3566aeaef499 Mon Sep 17 00:00:00 2001 From: Andreas Wachaja Date: Fri, 22 Apr 2016 13:54:20 +0200 Subject: [PATCH 38/66] velodyne_driver: Add dynamic_reconfigure and time_offset correction The value of time_offset is added to the calculated time stamp in live mode for each packet. --- velodyne_driver/CMakeLists.txt | 4 ++++ velodyne_driver/cfg/VelodyneNode.cfg | 14 ++++++++++++++ .../include/velodyne_driver/input.h | 6 +++--- velodyne_driver/package.xml | 2 ++ velodyne_driver/src/driver/CMakeLists.txt | 2 ++ velodyne_driver/src/driver/driver.cc | 19 +++++++++++++++++-- velodyne_driver/src/driver/driver.h | 11 +++++++++++ velodyne_driver/src/lib/input.cc | 10 +++++----- 8 files changed, 58 insertions(+), 10 deletions(-) create mode 100755 velodyne_driver/cfg/VelodyneNode.cfg diff --git a/velodyne_driver/CMakeLists.txt b/velodyne_driver/CMakeLists.txt index 319a4b4b5..e8901f234 100644 --- a/velodyne_driver/CMakeLists.txt +++ b/velodyne_driver/CMakeLists.txt @@ -3,6 +3,7 @@ project(velodyne_driver) set(${PROJECT_NAME}_CATKIN_DEPS diagnostic_updater + dynamic_reconfigure nodelet roscpp tf @@ -18,6 +19,9 @@ set(libpcap_LIBRARIES -lpcap) include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) +# Generate dynamic_reconfigure server +generate_dynamic_reconfigure_options(cfg/VelodyneNode.cfg) + # objects needed by other ROS packages that depend on this one catkin_package(CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} INCLUDE_DIRS include diff --git a/velodyne_driver/cfg/VelodyneNode.cfg b/velodyne_driver/cfg/VelodyneNode.cfg new file mode 100755 index 000000000..16196722a --- /dev/null +++ b/velodyne_driver/cfg/VelodyneNode.cfg @@ -0,0 +1,14 @@ +#!/usr/bin/env python +PACKAGE = "velodyne_driver" +NODE_NAME = "velodyne_node" +PARAMS_NAME = "VelodyneNode" + +from math import pi +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("time_offset", double_t, 0, "A manually calibrated offset (in seconds) to add to the timestamp before publication of a message.", + 0.0, -1.0, 1.0) + +exit(gen.generate(PACKAGE, NODE_NAME, PARAMS_NAME)) diff --git a/velodyne_driver/include/velodyne_driver/input.h b/velodyne_driver/include/velodyne_driver/input.h index adea8fab0..cc6823940 100644 --- a/velodyne_driver/include/velodyne_driver/input.h +++ b/velodyne_driver/include/velodyne_driver/input.h @@ -58,7 +58,7 @@ namespace velodyne_driver * -1 if end of file * > 0 if incomplete packet (is this possible?) */ - virtual int getPacket(velodyne_msgs::VelodynePacket *pkt) = 0; + virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset) = 0; /** @brief Set source IP, from where packets are accepted @@ -78,7 +78,7 @@ namespace velodyne_driver uint16_t udp_port = UDP_PORT_NUMBER); virtual ~InputSocket(); - virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); + virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset); void setDeviceIP( const std::string& ip ); private: @@ -103,7 +103,7 @@ namespace velodyne_driver double repeat_delay=0.0); virtual ~InputPCAP(); - virtual int getPacket(velodyne_msgs::VelodynePacket *pkt); + virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset); void setDeviceIP( const std::string& ip ); private: diff --git a/velodyne_driver/package.xml b/velodyne_driver/package.xml index e83160555..4779ef9ec 100644 --- a/velodyne_driver/package.xml +++ b/velodyne_driver/package.xml @@ -20,6 +20,7 @@ catkin diagnostic_updater + dynamic_reconfigure libpcap nodelet pluginlib @@ -32,6 +33,7 @@ rostest diagnostic_updater + dynamic_reconfigure libpcap nodelet pluginlib diff --git a/velodyne_driver/src/driver/CMakeLists.txt b/velodyne_driver/src/driver/CMakeLists.txt index 2102e3353..8d4179d7c 100644 --- a/velodyne_driver/src/driver/CMakeLists.txt +++ b/velodyne_driver/src/driver/CMakeLists.txt @@ -1,5 +1,6 @@ # build the driver node add_executable(velodyne_node velodyne_node.cc driver.cc) +add_dependencies(velodyne_node velodyne_driver_gencfg) target_link_libraries(velodyne_node velodyne_input ${catkin_LIBRARIES} @@ -8,6 +9,7 @@ target_link_libraries(velodyne_node # build the nodelet version add_library(driver_nodelet nodelet.cc driver.cc) +add_dependencies(driver_nodelet velodyne_driver_gencfg) target_link_libraries(driver_nodelet velodyne_input ${catkin_LIBRARIES} diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index 7bfa8eaa4..ee3eddcd7 100644 --- a/velodyne_driver/src/driver/driver.cc +++ b/velodyne_driver/src/driver/driver.cc @@ -81,6 +81,14 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, int udp_port; private_nh.param("port", udp_port, (int)UDP_PORT_NUMBER); + // Initialize dynamic reconfigure + srv_ = boost::make_shared > (private_nh); + dynamic_reconfigure::Server:: + CallbackType f; + f = boost::bind (&VelodyneDriver::callback, this, _1, _2); + srv_->setCallback (f); // Set callback function und call initially + // initialize diagnostics diagnostics_.setHardwareID(deviceName); const double diag_freq = packet_rate/config_.npackets; @@ -134,7 +142,7 @@ bool VelodyneDriver::poll(void) while (true) { // keep reading until full packet received - int rc = input_->getPacket(&scan->packets[i]); + int rc = input_->getPacket(&scan->packets[i], config_.time_offset); if (rc == 0) break; // got a full packet? if (rc < 0) return false; // end of file reached? } @@ -142,7 +150,7 @@ bool VelodyneDriver::poll(void) // publish message using time of last packet read ROS_DEBUG("Publishing a full Velodyne scan."); - scan->header.stamp = ros::Time(scan->packets[config_.npackets - 1].stamp); + scan->header.stamp = scan->packets[config_.npackets - 1].stamp; scan->header.frame_id = config_.frame_id; output_.publish(scan); @@ -154,4 +162,11 @@ bool VelodyneDriver::poll(void) return true; } +void VelodyneDriver::callback(velodyne_driver::VelodyneNodeConfig &config, + uint32_t level) +{ + ROS_INFO("Reconfigure Request"); + config_.time_offset = config.time_offset; +} + } // namespace velodyne_driver diff --git a/velodyne_driver/src/driver/driver.h b/velodyne_driver/src/driver/driver.h index 413b4bc51..6eda25a50 100644 --- a/velodyne_driver/src/driver/driver.h +++ b/velodyne_driver/src/driver/driver.h @@ -19,8 +19,10 @@ #include #include #include +#include #include +#include namespace velodyne_driver { @@ -37,6 +39,14 @@ class VelodyneDriver private: + ///Callback for dynamic reconfigure + void callback(velodyne_driver::VelodyneNodeConfig &config, + uint32_t level); + + ///Pointer to dynamic reconfigure service srv_ + boost::shared_ptr > srv_; + // configuration parameters struct { @@ -44,6 +54,7 @@ class VelodyneDriver std::string model; ///< device model name int npackets; ///< number of packets to collect double rpm; ///< device rotation rate (RPMs) + double time_offset; ///< time in seconds added to each velodyne time stamp } config_; boost::shared_ptr input_; diff --git a/velodyne_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index 383401d2a..885fd3b8e 100644 --- a/velodyne_driver/src/lib/input.cc +++ b/velodyne_driver/src/lib/input.cc @@ -91,7 +91,7 @@ namespace velodyne_driver } /** @brief Get one velodyne packet. */ - int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt) + int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset) { double time1 = ros::Time::now().toSec(); @@ -177,9 +177,9 @@ namespace velodyne_driver } // Average the times at which we begin and end reading. Use that to - // estimate when the scan occurred. + // estimate when the scan occurred. Add the time offset. double time2 = ros::Time::now().toSec(); - pkt->stamp = ros::Time((time2 + time1) / 2.0); + pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset); return 0; } @@ -248,7 +248,7 @@ namespace velodyne_driver } /** @brief Get one velodyne packet. */ - int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt) + int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset) { struct pcap_pkthdr *header; const u_char *pkt_data; @@ -267,7 +267,7 @@ namespace velodyne_driver packet_rate_.sleep(); memcpy(&pkt->data[0], pkt_data+42, packet_size); - pkt->stamp = ros::Time::now(); + pkt->stamp = ros::Time::now(); // time_offset not considered here, as no synchronization required empty_ = false; return 0; // success } From 8ca4f472858b3295242e6bc99009ee623a82d72d Mon Sep 17 00:00:00 2001 From: Ilya Date: Wed, 25 May 2016 18:50:07 -0400 Subject: [PATCH 39/66] update velodyne_driver package description to include all models --- velodyne_driver/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_driver/package.xml b/velodyne_driver/package.xml index 4779ef9ec..f288595f5 100644 --- a/velodyne_driver/package.xml +++ b/velodyne_driver/package.xml @@ -3,7 +3,7 @@ velodyne_driver 1.2.0 - ROS device driver for Velodyne HDL-64E, and HDL-32 LIDARs. + ROS device driver for Velodyne 3D LIDARs. Jack O'Quin Brice Rebsamen From 00db43dc6eb784d839cd2020220c3ac9cf8286eb Mon Sep 17 00:00:00 2001 From: Adam Stambler Date: Thu, 2 Jun 2016 16:56:51 -0400 Subject: [PATCH 40/66] Modified velodyne_pointcloud/src/conversion/colors.cc to remove address build warning for strict-aliasing. velodyne/velodyne_pointcloud/src/conversions/colors.cc:84:58: error: dereferencing type-punned pointer will break strict-aliasing rules [-Werror=strict-aliasing] p.rgb = *reinterpret_cast(&rainbow[color]); ^ --- velodyne_pointcloud/src/conversions/colors.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/src/conversions/colors.cc b/velodyne_pointcloud/src/conversions/colors.cc index 10878ab69..a0de9a27c 100644 --- a/velodyne_pointcloud/src/conversions/colors.cc +++ b/velodyne_pointcloud/src/conversions/colors.cc @@ -81,7 +81,7 @@ namespace velodyne_pointcloud // color lasers with the rainbow array int color = inMsg->points[i].ring % N_COLORS; - p.rgb = *reinterpret_cast(&rainbow[color]); + p.rgb = *reinterpret_cast(rainbow+color); outMsg->points.push_back(p); ++outMsg->width; From 252615c8fb733606db21230f2c28e131ea1e953d Mon Sep 17 00:00:00 2001 From: Adam Stambler Date: Thu, 2 Jun 2016 16:58:10 -0400 Subject: [PATCH 41/66] Modified velodyne_point_cloud/src/lib/rawdata.cc to address warning that last_azimuth_diff variable may be used uninitialized. Variable is now initialized to 0 at creation. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit velodyne/velodyne_pointcloud/src/lib/rawdata.cc:328:57: error: ‘last_azimuth_diff’ may be used uninitialized in this function [-Werror=maybe-uninitialized] azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION); --- velodyne_pointcloud/src/lib/rawdata.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index 9fafc8e29..bcf6dbdcd 100755 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -285,7 +285,7 @@ namespace velodyne_rawdata { float azimuth; float azimuth_diff; - float last_azimuth_diff; + float last_azimuth_diff=0; float azimuth_corrected_f; int azimuth_corrected; float x, y, z; From 075cb7626676e923e4d4fa0c58dcca9918170b69 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Wed, 15 Jun 2016 20:56:28 -0500 Subject: [PATCH 42/66] fix g++ 5.3.1 compile errors (#94) --- velodyne_driver/include/velodyne_driver/input.h | 9 +++++---- velodyne_driver/src/lib/input.cc | 9 +++++---- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/velodyne_driver/include/velodyne_driver/input.h b/velodyne_driver/include/velodyne_driver/input.h index 94aa6e0a3..1cf55fd9f 100644 --- a/velodyne_driver/include/velodyne_driver/input.h +++ b/velodyne_driver/include/velodyne_driver/input.h @@ -49,7 +49,7 @@ namespace velodyne_driver class Input { public: - Input(ros::NodeHandle private_nh, int port); + Input(ros::NodeHandle private_nh, uint16_t port); virtual ~Input() {} /** @brief Read one Velodyne packet. @@ -65,7 +65,7 @@ namespace velodyne_driver protected: ros::NodeHandle private_nh_; - int port_; + uint16_t port_; std::string devip_str_; }; @@ -74,7 +74,7 @@ namespace velodyne_driver { public: InputSocket(ros::NodeHandle private_nh, - uint16_t udp_port = UDP_PORT_NUMBER); + uint16_t port = DATA_PORT_NUMBER); virtual ~InputSocket(); virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, @@ -97,7 +97,8 @@ namespace velodyne_driver { public: InputPCAP(ros::NodeHandle private_nh, - double packet_rate, + uint16_t port = DATA_PORT_NUMBER, + double packet_rate = 0.0, std::string filename="", bool read_once=false, bool read_fast=false, diff --git a/velodyne_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index 15b2590f3..8ca5a573b 100644 --- a/velodyne_driver/src/lib/input.cc +++ b/velodyne_driver/src/lib/input.cc @@ -47,7 +47,7 @@ namespace velodyne_driver * @param private_nh ROS private handle for calling node. * @param port UDP port number. */ - Input::Input(ros::NodeHandle private_nh, int port): + Input::Input(ros::NodeHandle private_nh, uint16_t port): private_nh_(private_nh), port_(port) { @@ -66,7 +66,7 @@ namespace velodyne_driver * @param private_nh ROS private handle for calling node. * @param port UDP port number */ - InputSocket::InputSocket(ros::NodeHandle private_nh, int port): + InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port): Input(private_nh, port) { sockfd_ = -1; @@ -214,8 +214,9 @@ namespace velodyne_driver * @param packet_rate expected device packet frequency (Hz) * @param filename PCAP dump file name */ - InputPCAP::InputPCAP(ros::NodeHandle private_nh, int port, - double packet_rate, std::string filename): + InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, + double packet_rate, std::string filename, + bool read_once, bool read_fast, double repeat_delay): Input(private_nh, port), packet_rate_(packet_rate), filename_(filename) From 78fa2275067a3c027bb8931c1dc15634eef86647 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Fri, 17 Jun 2016 17:35:32 -0500 Subject: [PATCH 43/66] update README for Kinetic (#93) --- README.rst | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/README.rst b/README.rst index 9686124b9..db6f1e185 100644 --- a/README.rst +++ b/README.rst @@ -9,9 +9,10 @@ definition 3D LIDARs`_. The master branch normally contains code being tested for the next ROS release. It will not always work with every previous release. -The current ``master`` branch works with ROS Jade, Indigo and Hydro. It may -work with Groovy, but that is not guaranteed. To build for Fuerte -from source, check out the ``rosbuild`` branch instead of ``master``. +The current ``master`` branch works with ROS Kinetic, Jade, and +Indigo. It may work with Hydro and Groovy, but that has not been +tested recently. To build for Fuerte from source, check out the +``rosbuild`` branch instead of ``master``. .. _ROS: http://www.ros.org .. _Velodyne: http://www.ros.org/wiki/velodyne From f85ab35ab6e9c0e512c0108b9033deaf6a34f72e Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Wed, 29 Jun 2016 21:43:48 -0500 Subject: [PATCH 44/66] update change history --- velodyne_driver/CHANGELOG.rst | 7 ++++++- velodyne_pointcloud/CHANGELOG.rst | 4 +++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/velodyne_driver/CHANGELOG.rst b/velodyne_driver/CHANGELOG.rst index 82a41ba50..cbfe985d5 100644 --- a/velodyne_driver/CHANGELOG.rst +++ b/velodyne_driver/CHANGELOG.rst @@ -3,9 +3,14 @@ Change history 1.3.0 (forthcoming) ------------------- +* Use port number when reading PCAP data. +* Fix g++ 5.3.1 compiler errors. +* Add dynamic_reconfigure and time_offset correction. +* Make input destructors virtual. * Add VLP-16 support. * Add support for multiple devices. -* Contributors: Brice Rebsamen, ddillenberger, Patrick Hussey, Jack O'Quin +* Contributors: Brice Rebsamen, ddillenberger, Patrick Hussey, Andreas + Wachaja, Jack O'Quin 1.2.0 (2014-08-06) ------------------ diff --git a/velodyne_pointcloud/CHANGELOG.rst b/velodyne_pointcloud/CHANGELOG.rst index 80dd6231a..4e6b929fe 100644 --- a/velodyne_pointcloud/CHANGELOG.rst +++ b/velodyne_pointcloud/CHANGELOG.rst @@ -4,6 +4,8 @@ Change history 1.3.0 (forthcoming) ------------------- +* Fix compile warning for "Wrong initialization order". +* Fix unit tests for transform nodelet. * Provide dynamic reconfiguration for TransformNodelet (`#78`_). * Allow ``min_range`` as short as 0.4m. * Add VLP-16 support (`#44`_). @@ -12,7 +14,7 @@ Change history * Fix several calibration file problems (`#41`_, `#42`_). * Contributors: Bo Li, Patrick Hussey, William Woodall, Kun Li, lemiant, Jose Luis Blanco-Claraco, Alexander Schaefer, François - Pomerleau, Jack O'Quin + Pomerleau, Andreas Wachaja, Jack O'Quin 1.2.0 (2014-08-06) ------------------ From 58040a0f1b45a276f7106707dbfe9c127fd2bd84 Mon Sep 17 00:00:00 2001 From: Priyanka Dey Date: Thu, 30 Jun 2016 19:59:53 -0800 Subject: [PATCH 45/66] updated VLP-16 packet rate from user manual. Also verified with sensor. It reduced overlap in the pointcloud --- velodyne_driver/src/driver/driver.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index 8f005ff97..fbb8678ad 100644 --- a/velodyne_driver/src/driver/driver.cc +++ b/velodyne_driver/src/driver/driver.cc @@ -55,7 +55,7 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, } else if (config_.model == "VLP16") { - packet_rate = 781.25; // 300000 / 384 + packet_rate = 754; // 754 Packets/Second for Last or Strongest mode 1508 for dual (VLP-16 User Manual) model_full_name = "VLP-16"; } else From 374e19d81e1fad15931e14f46d5aeaf4ff4eb33e Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Tue, 5 Jul 2016 09:09:35 -0500 Subject: [PATCH 46/66] velodyne_driver: credit @priyankadey for VLP-16 bug fix (#96) --- velodyne_driver/CHANGELOG.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/velodyne_driver/CHANGELOG.rst b/velodyne_driver/CHANGELOG.rst index cbfe985d5..73d22633f 100644 --- a/velodyne_driver/CHANGELOG.rst +++ b/velodyne_driver/CHANGELOG.rst @@ -3,6 +3,7 @@ Change history 1.3.0 (forthcoming) ------------------- +* Correct VLP-16 packet rate error. * Use port number when reading PCAP data. * Fix g++ 5.3.1 compiler errors. * Add dynamic_reconfigure and time_offset correction. @@ -10,7 +11,7 @@ Change history * Add VLP-16 support. * Add support for multiple devices. * Contributors: Brice Rebsamen, ddillenberger, Patrick Hussey, Andreas - Wachaja, Jack O'Quin + Wachaja, priyankadey, Jack O'Quin 1.2.0 (2014-08-06) ------------------ From 2495c8f1526b9b8a3bce41bdda9df02f0b4a08a9 Mon Sep 17 00:00:00 2001 From: Matteo Murtas Date: Tue, 9 Aug 2016 17:40:12 +0200 Subject: [PATCH 47/66] velodyne_driver/src/lib/input.cc : fix for device_ip filter Fix for device_ip filter in InputSocket: initialization of devip_ for correct ip filtering in InputSocket::getPacket. --- velodyne_driver/src/lib/input.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/velodyne_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index 8ca5a573b..382b0bafe 100644 --- a/velodyne_driver/src/lib/input.cc +++ b/velodyne_driver/src/lib/input.cc @@ -70,6 +70,10 @@ namespace velodyne_driver Input(private_nh, port) { sockfd_ = -1; + + if (!devip_str_.empty()) { + inet_aton(devip_str_.c_str(),&devip_); + } // connect to Velodyne UDP port ROS_INFO_STREAM("Opening UDP socket: port " << port); From 3a19dcca856bbe947ac449cc2aa2dc05240237a1 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Thu, 11 Aug 2016 16:03:42 +0200 Subject: [PATCH 48/66] Fix misleading typecasts. intensity and VPoint::intensity are both of type float. --- velodyne_pointcloud/src/lib/rawdata.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index bcf6dbdcd..60199abec 100755 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -264,7 +264,7 @@ namespace velodyne_rawdata point.x = x_coord; point.y = y_coord; point.z = z_coord; - point.intensity = (uint8_t) intensity; + point.intensity = intensity; // append this point to the cloud pc.points.push_back(point); @@ -443,7 +443,7 @@ namespace velodyne_rawdata point.x = x_coord; point.y = y_coord; point.z = z_coord; - point.intensity = (uint8_t) intensity; + point.intensity = intensity; pc.points.push_back(point); ++pc.width; From 26c24ef3d3d64bc3c07d141cf276582abe7eb831 Mon Sep 17 00:00:00 2001 From: Daniel Jartoux Date: Thu, 11 Aug 2016 17:35:21 +0200 Subject: [PATCH 49/66] Add more options in launch files. - rpm, device_ip, port, read_once, read_fast, repeat_delay --- velodyne_driver/launch/nodelet_manager.launch | 5 +++++ velodyne_pointcloud/launch/32e_points.launch | 20 ++++++++++++++++++- .../launch/VLP16_points.launch | 16 ++++++++++++++- 3 files changed, 39 insertions(+), 2 deletions(-) diff --git a/velodyne_driver/launch/nodelet_manager.launch b/velodyne_driver/launch/nodelet_manager.launch index e17e52335..e8578e9ab 100644 --- a/velodyne_driver/launch/nodelet_manager.launch +++ b/velodyne_driver/launch/nodelet_manager.launch @@ -16,6 +16,9 @@ + + + @@ -25,6 +28,8 @@ + + diff --git a/velodyne_pointcloud/launch/32e_points.launch b/velodyne_pointcloud/launch/32e_points.launch index 7a5eb1165..04163851c 100644 --- a/velodyne_pointcloud/launch/32e_points.launch +++ b/velodyne_pointcloud/launch/32e_points.launch @@ -10,18 +10,36 @@ - + + + + + + + + + + + + + + + + + + + diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index 74b190a6b..aac55c762 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -8,15 +8,29 @@ - + + + + + + + + + + + + + + + From e3d7e74c4042eb3a113b9c9060217d56eae47422 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Tue, 16 Aug 2016 08:47:19 +0200 Subject: [PATCH 50/66] Remove unused constants. DISTANCE_MAX and DISTANCE_MAX_UNITS are not used anywhere in the code. Furthermore, using them would lead to errors as both VLP-64 manuals state that returns above 120 m should not be used. The VLP-32 manual allows 70 m as the maximum valid sensor range. --- .../include/velodyne_pointcloud/rawdata.h | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index 31100acbc..06205c969 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h @@ -46,15 +46,10 @@ namespace velodyne_rawdata static const int SCANS_PER_BLOCK = 32; static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); - static const float ROTATION_RESOLUTION = 0.01f; /**< degrees */ - static const uint16_t ROTATION_MAX_UNITS = 36000; /**< hundredths of degrees */ - - /** According to Bruce Hall DISTANCE_MAX is 65.0, but we noticed - * valid packets with readings up to 130.0. */ - static const float DISTANCE_MAX = 130.0f; /**< meters */ - static const float DISTANCE_RESOLUTION = 0.002f; /**< meters */ - static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX - / DISTANCE_RESOLUTION + 1.0); + static const float ROTATION_RESOLUTION = 0.01f; // [deg] + static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100] + static const float DISTANCE_RESOLUTION = 0.002f; // [m] + /** @todo make this work for both big and little-endian machines */ static const uint16_t UPPER_BANK = 0xeeff; static const uint16_t LOWER_BANK = 0xddff; From 0b0383290a75bb61330d822dd5d79c728270573c Mon Sep 17 00:00:00 2001 From: Daniel Jartoux Date: Tue, 16 Aug 2016 15:56:50 +0200 Subject: [PATCH 51/66] Rearranged alphabetically. --- velodyne_driver/launch/nodelet_manager.launch | 16 +++++++-------- velodyne_pointcloud/launch/32e_points.launch | 20 +++++++++---------- .../launch/VLP16_points.launch | 20 +++++++++---------- .../launch/cloud_nodelet.launch | 5 +++-- .../launch/transform_nodelet.launch | 8 ++++---- 5 files changed, 35 insertions(+), 34 deletions(-) diff --git a/velodyne_driver/launch/nodelet_manager.launch b/velodyne_driver/launch/nodelet_manager.launch index e8578e9ab..49433e223 100644 --- a/velodyne_driver/launch/nodelet_manager.launch +++ b/velodyne_driver/launch/nodelet_manager.launch @@ -9,27 +9,27 @@ + + - + + - - - + + - + + - - - diff --git a/velodyne_pointcloud/launch/32e_points.launch b/velodyne_pointcloud/launch/32e_points.launch index 04163851c..d326d472c 100644 --- a/velodyne_pointcloud/launch/32e_points.launch +++ b/velodyne_pointcloud/launch/32e_points.launch @@ -11,35 +11,35 @@ - + + + - + + - - - + + - + + - - - - + diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index aac55c762..76ad4e8c3 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -9,35 +9,35 @@ - + + + - + + - - - + + - + + - - - - + diff --git a/velodyne_pointcloud/launch/cloud_nodelet.launch b/velodyne_pointcloud/launch/cloud_nodelet.launch index a39bf80cb..12de2927b 100644 --- a/velodyne_pointcloud/launch/cloud_nodelet.launch +++ b/velodyne_pointcloud/launch/cloud_nodelet.launch @@ -8,12 +8,13 @@ - + + - + diff --git a/velodyne_pointcloud/launch/transform_nodelet.launch b/velodyne_pointcloud/launch/transform_nodelet.launch index ede8e376e..429d64de9 100644 --- a/velodyne_pointcloud/launch/transform_nodelet.launch +++ b/velodyne_pointcloud/launch/transform_nodelet.launch @@ -8,14 +8,14 @@ - - + + - - + + From 4d0d71f5cb3a8690f781aaf3efa5370e2e9506e2 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Sat, 8 Oct 2016 15:02:07 -0500 Subject: [PATCH 52/66] add launch args to support multiple devices (#108) --- velodyne_driver/launch/nodelet_manager.launch | 17 ++++++++--------- velodyne_pointcloud/launch/32e_points.launch | 11 ++++------- velodyne_pointcloud/launch/VLP16_points.launch | 9 ++++----- velodyne_pointcloud/launch/cloud_nodelet.launch | 12 ++++-------- .../launch/transform_nodelet.launch | 12 ++++-------- .../tests/cloud_nodelet_32e_hz.test | 12 +++--------- .../tests/cloud_nodelet_vlp16_hz.test | 12 +++--------- 7 files changed, 30 insertions(+), 55 deletions(-) diff --git a/velodyne_driver/launch/nodelet_manager.launch b/velodyne_driver/launch/nodelet_manager.launch index 49433e223..7b3186b09 100644 --- a/velodyne_driver/launch/nodelet_manager.launch +++ b/velodyne_driver/launch/nodelet_manager.launch @@ -1,16 +1,11 @@ - + - - + @@ -19,8 +14,12 @@ - + + + + + diff --git a/velodyne_pointcloud/launch/32e_points.launch b/velodyne_pointcloud/launch/32e_points.launch index d326d472c..4c4046695 100644 --- a/velodyne_pointcloud/launch/32e_points.launch +++ b/velodyne_pointcloud/launch/32e_points.launch @@ -1,11 +1,5 @@ - + @@ -13,6 +7,7 @@ + @@ -26,6 +21,7 @@ + @@ -38,6 +34,7 @@ + diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index 76ad4e8c3..d773f9e31 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -1,9 +1,5 @@ - + @@ -11,6 +7,7 @@ + @@ -24,6 +21,7 @@ + @@ -36,6 +34,7 @@ + diff --git a/velodyne_pointcloud/launch/cloud_nodelet.launch b/velodyne_pointcloud/launch/cloud_nodelet.launch index 12de2927b..049a89f51 100644 --- a/velodyne_pointcloud/launch/cloud_nodelet.launch +++ b/velodyne_pointcloud/launch/cloud_nodelet.launch @@ -1,18 +1,14 @@ - + + - + diff --git a/velodyne_pointcloud/launch/transform_nodelet.launch b/velodyne_pointcloud/launch/transform_nodelet.launch index 429d64de9..465f33fc2 100644 --- a/velodyne_pointcloud/launch/transform_nodelet.launch +++ b/velodyne_pointcloud/launch/transform_nodelet.launch @@ -1,18 +1,14 @@ - + + - + diff --git a/velodyne_pointcloud/tests/cloud_nodelet_32e_hz.test b/velodyne_pointcloud/tests/cloud_nodelet_32e_hz.test index 237374828..3f28ee127 100644 --- a/velodyne_pointcloud/tests/cloud_nodelet_32e_hz.test +++ b/velodyne_pointcloud/tests/cloud_nodelet_32e_hz.test @@ -3,18 +3,12 @@ - - - + + + - - - - - diff --git a/velodyne_pointcloud/tests/cloud_nodelet_vlp16_hz.test b/velodyne_pointcloud/tests/cloud_nodelet_vlp16_hz.test index 3bd6cd81c..55ff3b77e 100644 --- a/velodyne_pointcloud/tests/cloud_nodelet_vlp16_hz.test +++ b/velodyne_pointcloud/tests/cloud_nodelet_vlp16_hz.test @@ -3,18 +3,12 @@ - - - + + + - - - - - From ed6e94be662e4591a41de68d5bb8d78df80e9038 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Sun, 9 Oct 2016 11:14:20 -0500 Subject: [PATCH 53/66] test multiple nodelet manager support (#108) --- velodyne_pointcloud/tests/CMakeLists.txt | 4 +- .../tests/two_nodelet_managers.test | 48 +++++++++++++++++++ 2 files changed, 49 insertions(+), 3 deletions(-) create mode 100644 velodyne_pointcloud/tests/two_nodelet_managers.test diff --git a/velodyne_pointcloud/tests/CMakeLists.txt b/velodyne_pointcloud/tests/CMakeLists.txt index 85d173b5d..a8f7ac14e 100644 --- a/velodyne_pointcloud/tests/CMakeLists.txt +++ b/velodyne_pointcloud/tests/CMakeLists.txt @@ -44,11 +44,9 @@ add_rostest(cloud_node_64e_s2.1_hz.test) add_rostest(cloud_nodelet_64e_s2.1_hz.test) add_rostest(cloud_node_vlp16_hz.test) add_rostest(cloud_nodelet_vlp16_hz.test) - -## These tests don't work well enough to be worth the effort of -## running them: add_rostest(transform_node_hz.test) add_rostest(transform_nodelet_hz.test) +add_rostest(two_nodelet_managers.test) # parse check all the launch/*.launch files roslaunch_add_file_check(../launch) diff --git a/velodyne_pointcloud/tests/two_nodelet_managers.test b/velodyne_pointcloud/tests/two_nodelet_managers.test new file mode 100644 index 000000000..03b7c4183 --- /dev/null +++ b/velodyne_pointcloud/tests/two_nodelet_managers.test @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From d678c8798e0e9f0cd301d2eee0bf8e1fbb4d5ef1 Mon Sep 17 00:00:00 2001 From: Kevin Hallenbeck Date: Wed, 7 Dec 2016 19:01:21 -0500 Subject: [PATCH 54/66] Added velodyne_laserscan package and inserted into existing launch files --- velodyne/package.xml | 1 + velodyne_laserscan/CMakeLists.txt | 46 +++++ velodyne_laserscan/cfg/VelodyneLaserScan.cfg | 13 ++ velodyne_laserscan/nodelets.xml | 10 ++ velodyne_laserscan/package.xml | 33 ++++ velodyne_laserscan/src/VelodyneLaserScan.cpp | 162 ++++++++++++++++++ velodyne_laserscan/src/VelodyneLaserScan.h | 39 +++++ velodyne_laserscan/src/node.cpp | 17 ++ velodyne_laserscan/src/nodelet.cpp | 24 +++ velodyne_pointcloud/launch/32e_points.launch | 9 + .../launch/VLP16_points.launch | 9 + .../launch/laserscan_nodelet.launch | 14 ++ velodyne_pointcloud/package.xml | 1 + 13 files changed, 378 insertions(+) create mode 100644 velodyne_laserscan/CMakeLists.txt create mode 100755 velodyne_laserscan/cfg/VelodyneLaserScan.cfg create mode 100755 velodyne_laserscan/nodelets.xml create mode 100755 velodyne_laserscan/package.xml create mode 100644 velodyne_laserscan/src/VelodyneLaserScan.cpp create mode 100644 velodyne_laserscan/src/VelodyneLaserScan.h create mode 100644 velodyne_laserscan/src/node.cpp create mode 100644 velodyne_laserscan/src/nodelet.cpp create mode 100644 velodyne_pointcloud/launch/laserscan_nodelet.launch diff --git a/velodyne/package.xml b/velodyne/package.xml index 89511dd56..2688b1f08 100644 --- a/velodyne/package.xml +++ b/velodyne/package.xml @@ -18,6 +18,7 @@ catkin velodyne_driver + velodyne_laserscan velodyne_msgs velodyne_pointcloud diff --git a/velodyne_laserscan/CMakeLists.txt b/velodyne_laserscan/CMakeLists.txt new file mode 100644 index 000000000..a9dc4bc3d --- /dev/null +++ b/velodyne_laserscan/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 2.8.3) +project(velodyne_laserscan) + +find_package(catkin REQUIRED COMPONENTS + roscpp + nodelet + sensor_msgs + dynamic_reconfigure +) + +generate_dynamic_reconfigure_options( + cfg/VelodyneLaserScan.cfg +) + +catkin_package() + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/VelodyneLaserScan.cpp + src/nodelet.cpp +) +add_dependencies(${PROJECT_NAME} + ${PROJECT_NAME}_gencfg +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +add_executable(${PROJECT_NAME}_node + src/node.cpp +) +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ${PROJECT_NAME} +) + +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/velodyne_laserscan/cfg/VelodyneLaserScan.cfg b/velodyne_laserscan/cfg/VelodyneLaserScan.cfg new file mode 100755 index 000000000..deffd5fb4 --- /dev/null +++ b/velodyne_laserscan/cfg/VelodyneLaserScan.cfg @@ -0,0 +1,13 @@ +#! /usr/bin/env python + +PACKAGE='velodyne_laserscan' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# Name Type Lvl Description Default Min Max +gen.add("ring", int_t, 0, "Ring to extract as laser scan (-1 default)", -1, -1, 31) +gen.add("resolution", double_t, 0, "Laser scan angular resolution (rad)", 0.007, 0.001, 0.05) + +exit(gen.generate(PACKAGE, PACKAGE, "VelodyneLaserScan")) diff --git a/velodyne_laserscan/nodelets.xml b/velodyne_laserscan/nodelets.xml new file mode 100755 index 000000000..506ae9ad0 --- /dev/null +++ b/velodyne_laserscan/nodelets.xml @@ -0,0 +1,10 @@ + + + + Extract a single ring from a Velodyne PointCloud2 and publish + as a LaserScan. + + + diff --git a/velodyne_laserscan/package.xml b/velodyne_laserscan/package.xml new file mode 100755 index 000000000..aee781130 --- /dev/null +++ b/velodyne_laserscan/package.xml @@ -0,0 +1,33 @@ + + + velodyne_laserscan + 1.2.0 + + Extract a single ring of a Velodyne PointCloud2 and publish it as a LaserScan message + + Jack O'Quin + Micho Radovnikovich + Kevin Hallenbeck + BSD + + http://ros.org/wiki/velodyne_laserscan + https://github.com/ros-drivers/velodyne + https://github.com/ros-drivers/velodyne/issues + + catkin + + roscpp + nodelet + sensor_msgs + dynamic_reconfigure + + roscpp + nodelet + sensor_msgs + dynamic_reconfigure + + + + + + diff --git a/velodyne_laserscan/src/VelodyneLaserScan.cpp b/velodyne_laserscan/src/VelodyneLaserScan.cpp new file mode 100644 index 000000000..8abd60f01 --- /dev/null +++ b/velodyne_laserscan/src/VelodyneLaserScan.cpp @@ -0,0 +1,162 @@ +#include "VelodyneLaserScan.h" +#include + +namespace velodyne_laserscan { + +VelodyneLaserScan::VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv) : + ring_count_(0), nh_(nh), srv_(nh_priv) +{ + ros::SubscriberStatusCallback connect_cb = boost::bind(&VelodyneLaserScan::connectCb, this); + pub_ = nh.advertise("scan", 10, connect_cb, connect_cb); + + srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2)); +} + +void VelodyneLaserScan::connectCb() +{ + boost::lock_guard lock(connect_mutex_); + if (!pub_.getNumSubscribers()) { + sub_.shutdown(); + } else if (!sub_) { + sub_ = nh_.subscribe("velodyne_points", 10, &VelodyneLaserScan::recvCallback, this); + } +} + +void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg) +{ + // Latch ring count + if (!ring_count_) { + try { // Check for PointCloud2 field 'ring' + sensor_msgs::PointCloud2ConstIterator it(*msg, "ring"); + } catch (...) { + ROS_ERROR("VelodyneLaserScan: 'ring' field not present in PointCloud2"); + return; + } + for (sensor_msgs::PointCloud2ConstIterator it(*msg, "ring"); it != it.end(); ++it) { + const uint16_t ring = *it; + if (ring + 1 > ring_count_) { + ring_count_ = ring + 1; + } + } + if (ring_count_) { + ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_); + } else { + ROS_ERROR("VelodyneLaserScan: 'ring' field not present in PointCloud2"); + return; + } + } + + // Select ring to use + uint16_t ring; + if ((cfg_.ring < 0) || (cfg_.ring >= ring_count_)) { + // Default to ring closest to being level for each known sensor + if (ring_count_ > 32) { + ring = 57; // HDL-64E + } else if (ring_count_ > 16) { + ring = 23; // HDL-32E + } else { + ring = 8; // VLP-16 + } + } else { + ring = cfg_.ring; + } + ROS_INFO_ONCE("VelodyneLaserScan: Extracting ring %u", ring); + + // Load structure of PointCloud2 + int offset_x = -1; + int offset_y = -1; + int offset_z = -1; + int offset_i = -1; + int offset_r = -1; + for (size_t i = 0; i < msg->fields.size(); i++) { + if (msg->fields[i].name == "x") { + offset_x = msg->fields[i].offset; + } else if (msg->fields[i].name == "y") { + offset_y = msg->fields[i].offset; + } else if (msg->fields[i].name == "z") { + offset_z = msg->fields[i].offset; + } else if (msg->fields[i].name == "intensity") { + offset_i = msg->fields[i].offset; + } else if (msg->fields[i].name == "ring") { + offset_r = msg->fields[i].offset; + } + } + + // Construct LaserScan message + if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0)) { + const float RESOLUTION = fabsf(cfg_.resolution); + const size_t SIZE = M_PI / RESOLUTION; + sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan()); + scan->header = msg->header; + scan->angle_increment = RESOLUTION; + scan->angle_min = -M_PI; + scan->angle_max = M_PI; + scan->range_min = 0.0; + scan->range_max = 200.0; + scan->time_increment = 0.0; + scan->ranges.resize(SIZE, INFINITY); + if ((offset_x == 0) && (offset_y == 4) && (offset_z == 8) && (offset_i == 16) && (offset_r == 20)) { + scan->intensities.resize(SIZE); + for (sensor_msgs::PointCloud2ConstIterator it(*msg, "x"); it != it.end(); ++it) { + const uint16_t r = *((const uint16_t*)(&it[5])); // ring + if (r == ring) { + const float x = it[0]; // x + const float y = it[1]; // y + const float i = it[4]; // intensity + const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION; + if ((bin >= 0) && (bin < (int)SIZE)) { + scan->ranges[bin] = sqrtf(x * x + y * y); + scan->intensities[bin] = i; + } + } + } + } else { + ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method."); + if (offset_i >= 0) { + scan->intensities.resize(SIZE); + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_r(*msg, "ring"); + sensor_msgs::PointCloud2ConstIterator iter_i(*msg, "intensity"); + for ( ; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i) { + const uint16_t r = *iter_r; // ring + if (r == ring) { + const float x = *iter_x; // x + const float y = *iter_y; // y + const float i = *iter_i; // intensity + const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION; + if ((bin >= 0) && (bin < (int)SIZE)) { + scan->ranges[bin] = sqrtf(x * x + y * y); + scan->intensities[bin] = i; + } + } + } + } else { + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_r(*msg, "ring"); + for ( ; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_r) { + const uint16_t r = *iter_r; // ring + if (r == ring) { + const float x = *iter_x; // x + const float y = *iter_y; // y + const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION; + if ((bin >= 0) && (bin < (int)SIZE)) { + scan->ranges[bin] = sqrtf(x * x + y * y); + } + } + } + } + } + pub_.publish(scan); + } else { + ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)"); + } +} + +void VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level) +{ + cfg_ = config; +} + +} diff --git a/velodyne_laserscan/src/VelodyneLaserScan.h b/velodyne_laserscan/src/VelodyneLaserScan.h new file mode 100644 index 000000000..477348e50 --- /dev/null +++ b/velodyne_laserscan/src/VelodyneLaserScan.h @@ -0,0 +1,39 @@ +#ifndef VELODYNELASERSCAN_H +#define VELODYNELASERSCAN_H + +#include +#include +#include + +#include +#include + +#include +#include + +namespace velodyne_laserscan { + +class VelodyneLaserScan +{ +public: + VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv); + +private: + boost::mutex connect_mutex_; + void connectCb(); + void recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg); + + ros::NodeHandle nh_; + ros::Subscriber sub_; + ros::Publisher pub_; + + VelodyneLaserScanConfig cfg_; + dynamic_reconfigure::Server srv_; + void reconfig(VelodyneLaserScanConfig& config, uint32_t level); + + unsigned int ring_count_; +}; + +} + +#endif // VELODYNELASERSCAN_H diff --git a/velodyne_laserscan/src/node.cpp b/velodyne_laserscan/src/node.cpp new file mode 100644 index 000000000..6c95fb9ed --- /dev/null +++ b/velodyne_laserscan/src/node.cpp @@ -0,0 +1,17 @@ +#include +#include "VelodyneLaserScan.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "velodyne_laserscan_node"); + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + + // create VelodyneLaserScan class + velodyne_laserscan::VelodyneLaserScan n(nh, nh_priv); + + // handle callbacks until shut down + ros::spin(); + + return 0; +} diff --git a/velodyne_laserscan/src/nodelet.cpp b/velodyne_laserscan/src/nodelet.cpp new file mode 100644 index 000000000..e25129dce --- /dev/null +++ b/velodyne_laserscan/src/nodelet.cpp @@ -0,0 +1,24 @@ +#include +#include +#include +#include "VelodyneLaserScan.h" + +namespace velodyne_laserscan +{ + +class LaserScanNodelet: public nodelet::Nodelet +{ +public: + LaserScanNodelet() {} + ~LaserScanNodelet() {} + +private: + virtual void onInit() { + node_.reset(new VelodyneLaserScan(getNodeHandle(), getPrivateNodeHandle())); + } + boost::shared_ptr node_; +}; + +} + +PLUGINLIB_DECLARE_CLASS(velodyne_laserscan, LaserScanNodelet, velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet); diff --git a/velodyne_pointcloud/launch/32e_points.launch b/velodyne_pointcloud/launch/32e_points.launch index 4c4046695..c114f256e 100644 --- a/velodyne_pointcloud/launch/32e_points.launch +++ b/velodyne_pointcloud/launch/32e_points.launch @@ -16,6 +16,8 @@ + + @@ -39,4 +41,11 @@ + + + + + + + diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index d773f9e31..5d5b2374b 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -16,6 +16,8 @@ + + @@ -39,4 +41,11 @@ + + + + + + + diff --git a/velodyne_pointcloud/launch/laserscan_nodelet.launch b/velodyne_pointcloud/launch/laserscan_nodelet.launch new file mode 100644 index 000000000..7c5be247b --- /dev/null +++ b/velodyne_pointcloud/launch/laserscan_nodelet.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/velodyne_pointcloud/package.xml b/velodyne_pointcloud/package.xml index 6ae668136..e7e772762 100644 --- a/velodyne_pointcloud/package.xml +++ b/velodyne_pointcloud/package.xml @@ -48,6 +48,7 @@ velodyne_msgs yaml-cpp dynamic_reconfigure + velodyne_laserscan From e6560cc0841b51aa81b02eaa4e0bb75765b3704c Mon Sep 17 00:00:00 2001 From: Todor Stoyanov Date: Tue, 20 Dec 2016 15:38:16 +0100 Subject: [PATCH 55/66] Added an interface to set up raw data processing from a locally defined calibration file. This method is useful when processing data offline from a bag file, without starting any ros master --- .../include/velodyne_pointcloud/rawdata.h | 13 ++++++++ velodyne_pointcloud/src/lib/rawdata.cc | 32 +++++++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index 06205c969..02fa13c8e 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h @@ -133,6 +133,19 @@ namespace velodyne_rawdata */ int setup(ros::NodeHandle private_nh); + /** \brief Set up for data processing offline. + * Performs the same initialization as in setup, in the abscence of a ros::NodeHandle. + * this method is useful if unpacking data directly from bag files, without passing + * through a communication overhead. + * + * @param calibration_file path to the calibration file + * @param max_range_ cutoff for maximum range + * @param min_range_ cutoff for minimum range + * @returns 0 if successful; + * errno value for failure + */ + int setupOffline(std::string calibration_file, double max_range_, double min_range_); + void unpack(const velodyne_msgs::VelodynePacket &pkt, VPointCloud &pc); void setParameters(double min_range, double max_range, double view_direction, diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index 60199abec..77f370718 100755 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -106,6 +106,38 @@ namespace velodyne_rawdata return 0; } + + /** Set up for offline operation */ + int RawData::setupOffline(std::string calibration_file, double max_range_, double min_range_) + { + + config_.max_range = max_range_; + config_.min_range = min_range_; + ROS_INFO_STREAM("data ranges to publish: [" + << config_.min_range << ", " + << config_.max_range << "]"); + + config_.calibrationFile = calibration_file; + + ROS_INFO_STREAM("correction angles: " << config_.calibrationFile); + + calibration_.read(config_.calibrationFile); + if (!calibration_.initialized) { + ROS_ERROR_STREAM("Unable to open calibration file: " << + config_.calibrationFile); + return -1; + } + + // Set up cached values for sin and cos of all the possible headings + for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { + float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + cos_rot_table_[rot_index] = cosf(rotation); + sin_rot_table_[rot_index] = sinf(rotation); + } + return 0; + } + + /** @brief convert raw packet to point cloud * * @param pkt raw packet to unpack From 3747e937cfc1b2986c0b2e4983bdd0d7e8934079 Mon Sep 17 00:00:00 2001 From: Jack O'Quin Date: Wed, 19 Apr 2017 11:02:14 -0500 Subject: [PATCH 56/66] velodyne_pointcloud: remove incorrect catkin_package() DEPENDS option (#93) This eliminates a CMake warning when building on Xenial. --- velodyne_pointcloud/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index 3b2ea58d6..cefd96d72 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -45,7 +45,6 @@ include_directories(include ${catkin_INCLUDE_DIRS} catkin_package( CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS} - DEPENDS yaml-cpp python-yaml INCLUDE_DIRS include LIBRARIES velodyne_rawdata) From 00f407e3fb2b487d5c1e42444e804ccc8fc8f4b3 Mon Sep 17 00:00:00 2001 From: volkandre Date: Thu, 28 Sep 2017 09:55:03 -0400 Subject: [PATCH 57/66] Fixed bug. Laserscans now cover full 360 degrees. --- velodyne_laserscan/src/VelodyneLaserScan.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_laserscan/src/VelodyneLaserScan.cpp b/velodyne_laserscan/src/VelodyneLaserScan.cpp index 8abd60f01..cda68066d 100644 --- a/velodyne_laserscan/src/VelodyneLaserScan.cpp +++ b/velodyne_laserscan/src/VelodyneLaserScan.cpp @@ -85,7 +85,7 @@ void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg // Construct LaserScan message if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0)) { const float RESOLUTION = fabsf(cfg_.resolution); - const size_t SIZE = M_PI / RESOLUTION; + const size_t SIZE = 2.0 * M_PI / RESOLUTION; sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan()); scan->header = msg->header; scan->angle_increment = RESOLUTION; From 462c80930a094fd816dfee9c8478eb10d5f2c11f Mon Sep 17 00:00:00 2001 From: Kris Kozak Date: Thu, 28 Sep 2017 18:24:41 -0500 Subject: [PATCH 58/66] Add VLP16 Puck Hi-Res support --- .../params/VLP16_hires_db.yaml | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 velodyne_pointcloud/params/VLP16_hires_db.yaml diff --git a/velodyne_pointcloud/params/VLP16_hires_db.yaml b/velodyne_pointcloud/params/VLP16_hires_db.yaml new file mode 100644 index 000000000..395bd4333 --- /dev/null +++ b/velodyne_pointcloud/params/VLP16_hires_db.yaml @@ -0,0 +1,50 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0, + vert_correction: -0.17453292519943295, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0, + vert_correction: 0.011635528346628864, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0, + vert_correction: -0.15126186850617523, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0, + vert_correction: 0.03490658503988659, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0, + vert_correction: -0.1279908118129175, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0, + vert_correction: 0.05817764173314432, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0, + vert_correction: -0.10471975511965977, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0, + vert_correction: 0.08144869842640205, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0, + vert_correction: -0.08144869842640205, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0, + vert_correction: 0.10471975511965977, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0, + vert_correction: -0.05817764173314432, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0, + vert_correction: 0.1279908118129175, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0, + vert_correction: -0.03490658503988659, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0, + vert_correction: 0.15126186850617523, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0, + vert_correction: -0.011635528346628864, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0, + vert_correction: 0.17453292519943295, vert_offset_correction: 0.0} +num_lasers: 16 From d6fce8bd4963002cebb78204dd1689250a3808fb Mon Sep 17 00:00:00 2001 From: Kevin Hallenbeck Date: Wed, 11 Oct 2017 21:39:24 -0400 Subject: [PATCH 59/66] Package.xml format version 2 --- velodyne_laserscan/package.xml | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/velodyne_laserscan/package.xml b/velodyne_laserscan/package.xml index aee781130..9a506eb39 100755 --- a/velodyne_laserscan/package.xml +++ b/velodyne_laserscan/package.xml @@ -1,4 +1,4 @@ - + velodyne_laserscan 1.2.0 @@ -16,15 +16,10 @@ catkin - roscpp - nodelet - sensor_msgs - dynamic_reconfigure - - roscpp - nodelet - sensor_msgs - dynamic_reconfigure + roscpp + nodelet + sensor_msgs + dynamic_reconfigure From 345820eb5cd745bbbe6ae6cb2f5588f2391cddf8 Mon Sep 17 00:00:00 2001 From: Kevin Hallenbeck Date: Wed, 11 Oct 2017 21:44:58 -0400 Subject: [PATCH 60/66] Fixed validating PointCloud2 field types --- velodyne_laserscan/src/VelodyneLaserScan.cpp | 50 ++++++++++++-------- 1 file changed, 31 insertions(+), 19 deletions(-) diff --git a/velodyne_laserscan/src/VelodyneLaserScan.cpp b/velodyne_laserscan/src/VelodyneLaserScan.cpp index cda68066d..85990125b 100644 --- a/velodyne_laserscan/src/VelodyneLaserScan.cpp +++ b/velodyne_laserscan/src/VelodyneLaserScan.cpp @@ -26,10 +26,18 @@ void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg { // Latch ring count if (!ring_count_) { - try { // Check for PointCloud2 field 'ring' - sensor_msgs::PointCloud2ConstIterator it(*msg, "ring"); - } catch (...) { - ROS_ERROR("VelodyneLaserScan: 'ring' field not present in PointCloud2"); + // Check for PointCloud2 field 'ring' + bool found = false; + for (size_t i = 0; i < msg->fields.size(); i++) { + if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16) { + if (msg->fields[i].name == "ring") { + found = true; + break; + } + } + } + if (!found) { + ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2"); return; } for (sensor_msgs::PointCloud2ConstIterator it(*msg, "ring"); it != it.end(); ++it) { @@ -41,7 +49,7 @@ void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg if (ring_count_) { ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_); } else { - ROS_ERROR("VelodyneLaserScan: 'ring' field not present in PointCloud2"); + ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2"); return; } } @@ -69,16 +77,20 @@ void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg int offset_i = -1; int offset_r = -1; for (size_t i = 0; i < msg->fields.size(); i++) { - if (msg->fields[i].name == "x") { - offset_x = msg->fields[i].offset; - } else if (msg->fields[i].name == "y") { - offset_y = msg->fields[i].offset; - } else if (msg->fields[i].name == "z") { - offset_z = msg->fields[i].offset; - } else if (msg->fields[i].name == "intensity") { - offset_i = msg->fields[i].offset; - } else if (msg->fields[i].name == "ring") { - offset_r = msg->fields[i].offset; + if (msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32) { + if (msg->fields[i].name == "x") { + offset_x = msg->fields[i].offset; + } else if (msg->fields[i].name == "y") { + offset_y = msg->fields[i].offset; + } else if (msg->fields[i].name == "z") { + offset_z = msg->fields[i].offset; + } else if (msg->fields[i].name == "intensity") { + offset_i = msg->fields[i].offset; + } + } else if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16) { + if (msg->fields[i].name == "ring") { + offset_r = msg->fields[i].offset; + } } } @@ -114,11 +126,11 @@ void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method."); if (offset_i >= 0) { scan->intensities.resize(SIZE); + sensor_msgs::PointCloud2ConstIterator iter_r(*msg, "ring"); sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); - sensor_msgs::PointCloud2ConstIterator iter_r(*msg, "ring"); sensor_msgs::PointCloud2ConstIterator iter_i(*msg, "intensity"); - for ( ; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i) { + for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i) { const uint16_t r = *iter_r; // ring if (r == ring) { const float x = *iter_x; // x @@ -132,10 +144,10 @@ void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg } } } else { + sensor_msgs::PointCloud2ConstIterator iter_r(*msg, "ring"); sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); - sensor_msgs::PointCloud2ConstIterator iter_r(*msg, "ring"); - for ( ; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_r) { + for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r) { const uint16_t r = *iter_r; // ring if (r == ring) { const float x = *iter_x; // x From 269e83c8d718b0bed4fedbb5ebedf00a8d91d337 Mon Sep 17 00:00:00 2001 From: Kevin Hallenbeck Date: Wed, 11 Oct 2017 22:50:09 -0400 Subject: [PATCH 61/66] Added tests for velodyne_laserscan --- velodyne_laserscan/CMakeLists.txt | 5 + velodyne_laserscan/package.xml | 3 + velodyne_laserscan/tests/CMakeLists.txt | 30 + velodyne_laserscan/tests/lazy_subscriber.cpp | 90 +++ .../tests/lazy_subscriber_node.test | 17 + .../tests/lazy_subscriber_nodelet.test | 21 + velodyne_laserscan/tests/system.cpp | 582 ++++++++++++++++++ velodyne_laserscan/tests/system_node.test | 17 + velodyne_laserscan/tests/system_nodelet.test | 21 + 9 files changed, 786 insertions(+) create mode 100644 velodyne_laserscan/tests/CMakeLists.txt create mode 100644 velodyne_laserscan/tests/lazy_subscriber.cpp create mode 100644 velodyne_laserscan/tests/lazy_subscriber_node.test create mode 100644 velodyne_laserscan/tests/lazy_subscriber_nodelet.test create mode 100644 velodyne_laserscan/tests/system.cpp create mode 100644 velodyne_laserscan/tests/system_node.test create mode 100644 velodyne_laserscan/tests/system_nodelet.test diff --git a/velodyne_laserscan/CMakeLists.txt b/velodyne_laserscan/CMakeLists.txt index a9dc4bc3d..09e92b993 100644 --- a/velodyne_laserscan/CMakeLists.txt +++ b/velodyne_laserscan/CMakeLists.txt @@ -44,3 +44,8 @@ install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node install(FILES nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +if (CATKIN_ENABLE_TESTING) + add_subdirectory(tests) +endif() + diff --git a/velodyne_laserscan/package.xml b/velodyne_laserscan/package.xml index 9a506eb39..d76dc2527 100755 --- a/velodyne_laserscan/package.xml +++ b/velodyne_laserscan/package.xml @@ -21,6 +21,9 @@ sensor_msgs dynamic_reconfigure + roslaunch + rostest + diff --git a/velodyne_laserscan/tests/CMakeLists.txt b/velodyne_laserscan/tests/CMakeLists.txt new file mode 100644 index 000000000..3b23421c2 --- /dev/null +++ b/velodyne_laserscan/tests/CMakeLists.txt @@ -0,0 +1,30 @@ +### Unit tests +# +# Only configured when CATKIN_ENABLE_TESTING is true. + +# These dependencies are only needed for unit testing +find_package(roslaunch REQUIRED) +find_package(rostest REQUIRED) + +# C++ gtests +#catkin_add_gtest(test_calibration test_calibration.cpp) +#add_dependencies(test_calibration ${catkin_EXPORTED_TARGETS}) +#target_link_libraries(test_calibration velodyne_rawdata ${catkin_LIBRARIES}) + +# ROS rostests +add_rostest_gtest(test_lazy_subscriber_node lazy_subscriber_node.test lazy_subscriber.cpp) +add_dependencies(test_lazy_subscriber_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_lazy_subscriber_node ${catkin_LIBRARIES}) + +add_rostest_gtest(test_lazy_subscriber_nodelet lazy_subscriber_nodelet.test lazy_subscriber.cpp) +add_dependencies(test_lazy_subscriber_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_lazy_subscriber_nodelet ${catkin_LIBRARIES}) + +add_rostest_gtest(test_system_node system_node.test system.cpp) +add_dependencies(test_system_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_system_node ${catkin_LIBRARIES}) + +add_rostest_gtest(test_system_nodelet system_nodelet.test system.cpp) +add_dependencies(test_system_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_system_nodelet ${catkin_LIBRARIES}) + diff --git a/velodyne_laserscan/tests/lazy_subscriber.cpp b/velodyne_laserscan/tests/lazy_subscriber.cpp new file mode 100644 index 000000000..499180068 --- /dev/null +++ b/velodyne_laserscan/tests/lazy_subscriber.cpp @@ -0,0 +1,90 @@ +/********************************************************************* + * C++ unit test for velodyne_laserscan + * Verify correct handling of subscribe and unsubscribe events + *********************************************************************/ + +#include + +#include +#include +#include + +// Subscriber receive callback +void recv(const sensor_msgs::LaserScanConstPtr& msg) {} + +// Build and publish a minimal PointCloud2 message +void publish(const ros::Publisher &pub) { + const uint32_t POINT_STEP = 32; + sensor_msgs::PointCloud2 msg; + msg.header.frame_id = ""; + msg.header.stamp = ros::Time::now(); + msg.fields.resize(5); + msg.fields[0].name = "x"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 4; + msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].count = 1; + msg.fields[2].name = "z"; + msg.fields[2].offset = 8; + msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].count = 1; + msg.fields[3].name = "intensity"; + msg.fields[3].offset = 16; + msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[3].count = 1; + msg.fields[4].name = "ring"; + msg.fields[4].offset = 20; + msg.fields[4].datatype = sensor_msgs::PointField::UINT16; + msg.fields[4].count = 1; + msg.data.resize(1 * POINT_STEP, 0x00); + msg.point_step = POINT_STEP; + msg.row_step = msg.data.size(); + msg.height = 1; + msg.width = msg.row_step / POINT_STEP; + msg.is_bigendian = false; + msg.is_dense = true; + pub.publish(msg); +} + +// Verify correct handling of subscribe and unsubscribe events +TEST(Main, subscribe_unsubscribe) +{ + ros::NodeHandle nh; + ros::Publisher pub = nh.advertise("velodyne_points", 2); + + // Wait for node to startup + ros::WallDuration(2.0).sleep(); + ros::spinOnce(); + EXPECT_EQ(0, pub.getNumSubscribers()); + + // Subscribe to 'scan' and expect the node to subscribe to 'velodyne_points' + ros::Subscriber sub = nh.subscribe("scan", 2, recv); + for (size_t i = 10; i > 0; i--) { + publish(pub); + ros::WallDuration(0.1).sleep(); + ros::spinOnce(); + } + EXPECT_EQ(1, sub.getNumPublishers()); + EXPECT_EQ(1, pub.getNumSubscribers()); + + // Unsubscribe from 'scan' and expect the node to unsubscribe from 'velodyne_points' + sub.shutdown(); + for (size_t i = 10; i > 0; i--) { + publish(pub); + ros::WallDuration(0.1).sleep(); + ros::spinOnce(); + } + EXPECT_EQ(0, sub.getNumPublishers()); + EXPECT_EQ(0, pub.getNumSubscribers()); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_lazy_subscriber"); + return RUN_ALL_TESTS(); +} diff --git a/velodyne_laserscan/tests/lazy_subscriber_node.test b/velodyne_laserscan/tests/lazy_subscriber_node.test new file mode 100644 index 000000000..f852b0b32 --- /dev/null +++ b/velodyne_laserscan/tests/lazy_subscriber_node.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + diff --git a/velodyne_laserscan/tests/lazy_subscriber_nodelet.test b/velodyne_laserscan/tests/lazy_subscriber_nodelet.test new file mode 100644 index 000000000..787a3a28b --- /dev/null +++ b/velodyne_laserscan/tests/lazy_subscriber_nodelet.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + diff --git a/velodyne_laserscan/tests/system.cpp b/velodyne_laserscan/tests/system.cpp new file mode 100644 index 000000000..a96613e6e --- /dev/null +++ b/velodyne_laserscan/tests/system.cpp @@ -0,0 +1,582 @@ +/********************************************************************* + * C++ unit test for velodyne_laserscan + * Verify all aspects of the system + *********************************************************************/ + +#include + +#include +#include +#include + +// Define our own PointCloud type for easy use +typedef struct { + float x; // x + float y; // y + float z; // z + float i; // intensity + uint16_t r; // ring +} Point; +typedef struct { + std_msgs::Header header; + std::vector points; +} PointCloud; + +// Global variables +ros::Publisher g_pub; +ros::Subscriber g_sub; +sensor_msgs::LaserScan g_scan; +volatile bool g_scan_new = false; + +// Convert WallTime to Time +static inline ros::Time rosTime(const ros::WallTime &stamp) { + return ros::Time(stamp.sec, stamp.nsec); +} + +// Subscriber receive callback +void recv(const sensor_msgs::LaserScanConstPtr& msg) { + g_scan = *msg; + g_scan_new = true; +} + +// Wait for incoming LaserScan message +bool waitForScan(ros::WallDuration dur) { + const ros::WallTime start = ros::WallTime::now(); + while (!g_scan_new) { + if ((ros::WallTime::now() - start) > dur) { + return false; + } + ros::WallDuration(0.001).sleep(); + ros::spinOnce(); + } + return true; +} + +// Build and publish PointCloud2 messages of various structures +void publishXYZIR1(const PointCloud &cloud) { + g_scan_new = false; + const uint32_t POINT_STEP = 32; + sensor_msgs::PointCloud2 msg; + msg.header.frame_id = cloud.header.frame_id; + msg.header.stamp = cloud.header.stamp; + msg.fields.resize(5); + msg.fields[0].name = "x"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 4; + msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].count = 1; + msg.fields[2].name = "z"; + msg.fields[2].offset = 8; + msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].count = 1; + msg.fields[3].name = "intensity"; + msg.fields[3].offset = 16; + msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[3].count = 1; + msg.fields[4].name = "ring"; + msg.fields[4].offset = 20; + msg.fields[4].datatype = sensor_msgs::PointField::UINT16; + msg.fields[4].count = 1; + msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00); + msg.point_step = POINT_STEP; + msg.row_step = msg.data.size(); + msg.height = 1; + msg.width = msg.row_step / POINT_STEP; + msg.is_bigendian = false; + msg.is_dense = true; + uint8_t *ptr = msg.data.data(); + for (size_t i = 0; i < cloud.points.size(); i++) { + *((float*)(ptr + 0)) = cloud.points[i].x; + *((float*)(ptr + 4)) = cloud.points[i].y; + *((float*)(ptr + 8)) = cloud.points[i].z; + *((float*)(ptr + 16)) = cloud.points[i].i; + *((uint16_t*)(ptr + 20)) = cloud.points[i].r; + ptr += POINT_STEP; + } + g_pub.publish(msg); +} +void publishXYZIR2(const PointCloud &cloud) { + g_scan_new = false; + const uint32_t POINT_STEP = 19; + sensor_msgs::PointCloud2 msg; + msg.header.frame_id = cloud.header.frame_id; + msg.header.stamp = cloud.header.stamp; + msg.fields.resize(5); + msg.fields[0].name = "z"; + msg.fields[0].offset = 4; + msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 8; + msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].count = 1; + msg.fields[2].name = "x"; + msg.fields[2].offset = 12; + msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].count = 1; + msg.fields[3].name = "intensity"; + msg.fields[3].offset = 0; + msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[3].count = 1; + msg.fields[4].name = "ring"; + msg.fields[4].offset = 16; + msg.fields[4].datatype = sensor_msgs::PointField::UINT16; + msg.fields[4].count = 1; + msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00); + msg.point_step = POINT_STEP; + msg.row_step = msg.data.size(); + msg.height = 1; + msg.width = msg.row_step / POINT_STEP; + msg.is_bigendian = false; + msg.is_dense = true; + uint8_t *ptr = msg.data.data(); + for (size_t i = 0; i < cloud.points.size(); i++) { + *((float*)(ptr + 0)) = cloud.points[i].i; + *((float*)(ptr + 4)) = cloud.points[i].z; + *((float*)(ptr + 8)) = cloud.points[i].y; + *((float*)(ptr + 12)) = cloud.points[i].x; + *((uint16_t*)(ptr + 16)) = cloud.points[i].r; + ptr += POINT_STEP; + } + g_pub.publish(msg); +} +void publishXYZR(const PointCloud &cloud) { + g_scan_new = false; + const uint32_t POINT_STEP = 15; + sensor_msgs::PointCloud2 msg; + msg.header.frame_id = cloud.header.frame_id; + msg.header.stamp = cloud.header.stamp; + msg.fields.resize(4); + msg.fields[0].name = "x"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 4; + msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].count = 1; + msg.fields[2].name = "z"; + msg.fields[2].offset = 8; + msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].count = 1; + msg.fields[3].name = "ring"; + msg.fields[3].offset = 12; + msg.fields[3].datatype = sensor_msgs::PointField::UINT16; + msg.fields[3].count = 1; + msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00); + msg.point_step = POINT_STEP; + msg.row_step = msg.data.size(); + msg.height = 1; + msg.width = msg.row_step / POINT_STEP; + msg.is_bigendian = false; + msg.is_dense = true; + uint8_t *ptr = msg.data.data(); + for (size_t i = 0; i < cloud.points.size(); i++) { + *((float*)(ptr + 0)) = cloud.points[i].x; + *((float*)(ptr + 4)) = cloud.points[i].y; + *((float*)(ptr + 8)) = cloud.points[i].z; + *((uint16_t*)(ptr + 12)) = cloud.points[i].r; + ptr += POINT_STEP; + } + g_pub.publish(msg); +} +void publishR(const PointCloud &cloud) { + g_scan_new = false; + const uint32_t POINT_STEP = 2; + sensor_msgs::PointCloud2 msg; + msg.header.stamp = rosTime(ros::WallTime::now()); + msg.fields.resize(1); + msg.fields[0].name = "ring"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = sensor_msgs::PointField::UINT16; + msg.fields[0].count = 1; + msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00); + msg.point_step = POINT_STEP; + msg.row_step = msg.data.size(); + msg.height = 1; + msg.width = msg.row_step / POINT_STEP; + uint8_t *ptr = msg.data.data(); + for (size_t i = 0; i < cloud.points.size(); i++) { + *((uint16_t*)(ptr + 0)) = cloud.points[i].r; + ptr += POINT_STEP; + } + g_pub.publish(msg); +} +void publishXYZR32(const PointCloud &cloud) { + g_scan_new = false; + const uint32_t POINT_STEP = 16; + sensor_msgs::PointCloud2 msg; + msg.header.frame_id = cloud.header.frame_id; + msg.header.stamp = cloud.header.stamp; + msg.fields.resize(4); + msg.fields[0].name = "x"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 4; + msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].count = 1; + msg.fields[2].name = "z"; + msg.fields[2].offset = 8; + msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].count = 1; + msg.fields[3].name = "ring"; + msg.fields[3].offset = 12; + msg.fields[3].datatype = sensor_msgs::PointField::UINT32; + msg.fields[3].count = 1; + msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00); + msg.point_step = POINT_STEP; + msg.row_step = msg.data.size(); + msg.height = 1; + msg.width = msg.row_step / POINT_STEP; + msg.is_bigendian = false; + msg.is_dense = true; + uint8_t *ptr = msg.data.data(); + for (size_t i = 0; i < cloud.points.size(); i++) { + *((float*)(ptr + 0)) = cloud.points[i].x; + *((float*)(ptr + 4)) = cloud.points[i].y; + *((float*)(ptr + 8)) = cloud.points[i].z; + *((uint32_t*)(ptr + 12)) = cloud.points[i].r; + ptr += POINT_STEP; + } + g_pub.publish(msg); +} +void publishXYZ(const PointCloud &cloud) { + g_scan_new = false; + const uint32_t POINT_STEP = 12; + sensor_msgs::PointCloud2 msg; + msg.header.stamp = rosTime(ros::WallTime::now()); + msg.fields.resize(3); + msg.fields[0].name = "x"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 4; + msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[1].count = 1; + msg.fields[2].name = "z"; + msg.fields[2].offset = 8; + msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + msg.fields[2].count = 1; + msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00); + msg.point_step = POINT_STEP; + msg.row_step = msg.data.size(); + msg.height = 1; + msg.width = msg.row_step / POINT_STEP; + uint8_t *ptr = msg.data.data(); + for (size_t i = 0; i < cloud.points.size(); i++) { + *((float*)(ptr + 0)) = cloud.points[i].x; + *((float*)(ptr + 4)) = cloud.points[i].y; + *((float*)(ptr + 8)) = cloud.points[i].z; + ptr += POINT_STEP; + } + g_pub.publish(msg); +} +void publishNone() { + g_scan_new = false; + const uint32_t POINT_STEP = 16; + sensor_msgs::PointCloud2 msg; + msg.header.stamp = rosTime(ros::WallTime::now()); + msg.data.resize(1 * POINT_STEP, 0x00); + msg.point_step = POINT_STEP; + msg.row_step = msg.data.size(); + msg.height = 1; + msg.width = msg.row_step / POINT_STEP; + g_pub.publish(msg); +} + +// Find the index of the point in the PointCloud with the shortest 2d distance to the point (x,y) +static inline float SQUARE(float x) { return x * x; } +size_t findClosestIndex(const PointCloud &cloud, uint16_t ring, float x, float y) { + size_t index = SIZE_MAX; + float delta = INFINITY; + for (size_t i = 0; i < cloud.points.size(); i++) { + if (cloud.points[i].r == ring) { + float dist = SQUARE(x - cloud.points[i].x) + SQUARE(y - cloud.points[i].y); + if (dist < delta) { + delta = dist; + index = i; + } + } + } + return index; +} + +// Verify that all LaserScan header values are values are passed through, and other values are default +void verifyScanEmpty(const PointCloud &cloud, bool intensity = true) { + ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp); + EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id); + for (size_t i = 0; i < g_scan.ranges.size(); i++) { + EXPECT_EQ(INFINITY, g_scan.ranges[i]); + } + if (!intensity) { + EXPECT_EQ(0, g_scan.intensities.size()); + } else { + EXPECT_EQ(g_scan.ranges.size(), g_scan.intensities.size()); + for (size_t i = 0; i < g_scan.intensities.size(); i++) { + EXPECT_EQ(0.0, g_scan.intensities[i]); + } + } +} + +// Verify that every PointCloud point made it to the LaserScan and other values are default +void verifyScanSparse(const PointCloud &cloud, uint16_t ring, uint16_t ring_count, bool intensity = true) { + ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp); + EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id); + EXPECT_EQ(intensity ? g_scan.ranges.size() : 0, g_scan.intensities.size()); + size_t count = 0; + for (size_t i = 0; i < g_scan.ranges.size(); i++) { + double r = g_scan.ranges[i]; + if (std::isfinite(r)) { + float a = g_scan.angle_min + i * g_scan.angle_increment; + float x = g_scan.ranges[i] * cosf(a); + float y = g_scan.ranges[i] * sinf(a); + float e = g_scan.ranges[i] * g_scan.angle_increment + (float)1e-3; // allowable error + size_t index = findClosestIndex(cloud, ring, x, y); + if (index < cloud.points.size()) { + count++; + EXPECT_NEAR(cloud.points[index].x, x, e); + EXPECT_NEAR(cloud.points[index].y, y, e); + if (i < g_scan.intensities.size()) { + EXPECT_EQ(cloud.points[index].i, g_scan.intensities[i]); + } + } else { + EXPECT_TRUE(false); // LaserScan point not found in PointCloud + } + } else { + EXPECT_EQ(INFINITY, r); + } + } + if (ring_count > 0) { + EXPECT_EQ(cloud.points.size() / ring_count, count); // Make sure that all points were converted to ranges + } +} + +// Verify that every LaserScan point is not default, and every point came from the PointCloud +void verifyScanDense(const PointCloud &cloud, uint16_t ring, bool intensity = true) { + ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp); + EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id); + EXPECT_EQ(intensity ? g_scan.ranges.size() : 0, g_scan.intensities.size()); + for (size_t i = 0; i < g_scan.ranges.size(); i++) { + double r = g_scan.ranges[i]; + if (std::isfinite(r)) { + float a = g_scan.angle_min + i * g_scan.angle_increment; + float x = g_scan.ranges[i] * cosf(a); + float y = g_scan.ranges[i] * sinf(a); + float e = g_scan.ranges[i] * g_scan.angle_increment + (float)1e-3; // allowable error + size_t index = findClosestIndex(cloud, ring, x, y); + if (index < cloud.points.size()) { + EXPECT_NEAR(cloud.points[index].x, x, e); + EXPECT_NEAR(cloud.points[index].y, y, e); + ///@TODO: Test for matching intensity + } else { + EXPECT_TRUE(false); // LaserScan point not found in PointCloud + } + } else { + EXPECT_TRUE(false); // Dense PointCloud should populate every range in LaserScan + } + } +} + +// Verify that no LaserScan is generated when the PointCloud2 message is missing required fields +TEST(System, missing_fields) +{ + // Make sure system is connected + ASSERT_EQ(1, g_sub.getNumPublishers()); + ASSERT_EQ(1, g_pub.getNumSubscribers()); + + // Create PointCloud with 16 rings + PointCloud cloud; + cloud.points.resize(1); + cloud.points[0].x = 0.0; + cloud.points[0].y = 0.0; + cloud.points[0].z = 0.0; + cloud.points[0].i = 0.0; + cloud.points[0].r = 15; + + // Verify no LaserScan when PointCloud2 fields are empty + publishNone(); + EXPECT_FALSE(waitForScan(ros::WallDuration(0.5))); + + // Verify no LaserScan when PointCloud2 fields are missing 'ring' + publishXYZ(cloud); + EXPECT_FALSE(waitForScan(ros::WallDuration(0.5))); + + // Verify no LaserScan when PointCloud2 field 'ring' is the incorrect type + publishXYZR32(cloud); + EXPECT_FALSE(waitForScan(ros::WallDuration(0.5))); + + + // Verify no LaserScan when PointCloud2 fields are missing 'x' and 'y' + publishR(cloud); + EXPECT_FALSE(waitForScan(ros::WallDuration(0.5))); + + // Verify that the node hasn't crashed by sending normal PointCloud2 fields + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZIR1(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp); +} + +// Verify that non-point fields are passed through unmodified +TEST(System, empty_data) +{ + // Make sure system is connected + ASSERT_EQ(1, g_sub.getNumPublishers()); + ASSERT_EQ(1, g_pub.getNumSubscribers()); + + // Create PointCloud with 16 rings + PointCloud cloud; + cloud.header.frame_id = "abcdefghijklmnopqrstuvwxyz"; + cloud.points.resize(1); + cloud.points[0].x = 0.0; + cloud.points[0].y = 0.0; + cloud.points[0].z = 0.0; + cloud.points[0].i = 0.0; + cloud.points[0].r = 15; + + // Verify that all three PointCloud2 types create proper default values + + // PointXYZIR (expected format) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZIR1(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanEmpty(cloud, true); + + // PointXYZIR (unexpected format with intensity) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZIR2(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanEmpty(cloud, true); + + // PointXYZR (unexpected format without intensity) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZR(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanEmpty(cloud, false); +} + +// Verify that every piece of a small amount of random data is passed through +TEST(System, random_data_sparse) +{ + // Make sure system is connected + ASSERT_EQ(1, g_sub.getNumPublishers()); + ASSERT_EQ(1, g_pub.getNumSubscribers()); + + // Create PointCloud with sparse random data + PointCloud cloud; + cloud.header.frame_id = "velodyne"; + const size_t RANGE_COUNT = 100; + const size_t RING_COUNT = 16; + const double RANGE_MAX = 20.0; + const double INTENSITY_MAX = 1.0; + for (size_t i = 0; i < RANGE_COUNT; i++) { + double angle_y = i * 1.99 * M_PI / RANGE_COUNT; // yaw + for (size_t j = 0; j < RING_COUNT; j++) { + double angle_p = j * 0.2 * M_PI / RING_COUNT - 0.1 * M_PI; // pitch + double range = rand() * (RANGE_MAX / RAND_MAX); + Point point; + point.x = range * cos(angle_p) * cos(angle_y); + point.y = range * cos(angle_p) * sin(angle_y); + point.z = range * sin(angle_p); + point.i = rand() * (INTENSITY_MAX / RAND_MAX); + point.r = j; + cloud.points.push_back(point); + } + } + + // Verify that all three PointCloud2 types are handled correctly + + // PointXYZIR (expected format) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZIR1(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanSparse(cloud, 8, RING_COUNT, true); + + // PointXYZIR (unexpected format with intensity) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZIR2(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanSparse(cloud, 8, RING_COUNT, true); + + // PointXYZR (unexpected format without intensity) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZR(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanSparse(cloud, 8, RING_COUNT, false); +} + +// Verify that every LaserScan range is valid when given an extra large amount of random data +TEST(System, random_data_dense) +{ + // Make sure system is connected + ASSERT_EQ(1, g_sub.getNumPublishers()); + ASSERT_EQ(1, g_pub.getNumSubscribers()); + + // Create PointCloud with dense random data + PointCloud cloud; + cloud.header.frame_id = "velodyne"; + const size_t RANGE_COUNT = 2500; + const size_t RING_COUNT = 16; + const double RANGE_MAX = 20.0; + const double INTENSITY_MAX = 1.0; + for (size_t i = 0; i < RANGE_COUNT; i++) { + double angle_y = i * 2.0 * M_PI / RANGE_COUNT; // yaw + for (size_t j = 0; j < RING_COUNT; j++) { + double angle_p = j * 0.2 * M_PI / RING_COUNT - 0.1 * M_PI; // pitch + double range = rand() * (RANGE_MAX / RAND_MAX); + Point point; + point.x = range * cos(angle_p) * cos(angle_y); + point.y = range * cos(angle_p) * sin(angle_y); + point.z = range * sin(angle_p); + point.i = rand() * (INTENSITY_MAX / RAND_MAX); + point.r = j; + cloud.points.push_back(point); + } + } + + // Verify that all three PointCloud2 types are handled correctly + + // PointXYZIR (expected format) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZIR1(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanDense(cloud, 8, true); + + // PointXYZIR (unexpected format with intensity) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZIR2(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanDense(cloud, 8, true); + + // PointXYZR (unexpected format without intensity) + cloud.header.stamp = rosTime(ros::WallTime::now()); + publishXYZR(cloud); + ASSERT_TRUE(waitForScan(ros::WallDuration(1.0))); + verifyScanDense(cloud, 8, false); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + // Initialize ROS + ros::init(argc, argv, "test_lazy_subscriber"); + ros::NodeHandle nh; + + // Setup publisher and subscriber + g_pub = nh.advertise("velodyne_points", 2); + g_sub = nh.subscribe("scan", 2, recv); + + // Wait for other nodes to startup + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + + // Run all the tests that were declared with TEST() + return RUN_ALL_TESTS(); +} diff --git a/velodyne_laserscan/tests/system_node.test b/velodyne_laserscan/tests/system_node.test new file mode 100644 index 000000000..05daf8bf8 --- /dev/null +++ b/velodyne_laserscan/tests/system_node.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + diff --git a/velodyne_laserscan/tests/system_nodelet.test b/velodyne_laserscan/tests/system_nodelet.test new file mode 100644 index 000000000..55a1e991e --- /dev/null +++ b/velodyne_laserscan/tests/system_nodelet.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + From cccc1387ceb42fab97d01034dfd14e1a56da6691 Mon Sep 17 00:00:00 2001 From: Kevin Hallenbeck Date: Wed, 1 Nov 2017 18:12:23 -0400 Subject: [PATCH 62/66] Update to use non deprecated pluginlib macro --- velodyne_driver/src/driver/nodelet.cc | 5 ++--- velodyne_pointcloud/src/conversions/cloud_nodelet.cc | 7 +++---- velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc | 7 +++---- velodyne_pointcloud/src/conversions/transform_nodelet.cc | 8 +++----- 4 files changed, 11 insertions(+), 16 deletions(-) diff --git a/velodyne_driver/src/driver/nodelet.cc b/velodyne_driver/src/driver/nodelet.cc index 6e9966b63..f2dfca2b1 100644 --- a/velodyne_driver/src/driver/nodelet.cc +++ b/velodyne_driver/src/driver/nodelet.cc @@ -81,6 +81,5 @@ void DriverNodelet::devicePoll() // Register this plugin with pluginlib. Names must match nodelet_velodyne.xml. // -// parameters are: package, class name, class type, base class type -PLUGINLIB_DECLARE_CLASS(velodyne_driver, DriverNodelet, - velodyne_driver::DriverNodelet, nodelet::Nodelet); +// parameters are: class type, base class type +PLUGINLIB_EXPORT_CLASS(velodyne_driver::DriverNodelet, nodelet::Nodelet) diff --git a/velodyne_pointcloud/src/conversions/cloud_nodelet.cc b/velodyne_pointcloud/src/conversions/cloud_nodelet.cc index f762b1298..0ddd5f705 100644 --- a/velodyne_pointcloud/src/conversions/cloud_nodelet.cc +++ b/velodyne_pointcloud/src/conversions/cloud_nodelet.cc @@ -42,8 +42,7 @@ namespace velodyne_pointcloud } // namespace velodyne_pointcloud -// Register this plugin with pluginlib. Names must match nodelet_velodyne.xml. +// Register this plugin with pluginlib. Names must match nodelets.xml. // -// parameters: package, class name, class type, base class type -PLUGINLIB_DECLARE_CLASS(velodyne_pointcloud, CloudNodelet, - velodyne_pointcloud::CloudNodelet, nodelet::Nodelet); +// parameters: class type, base class type +PLUGINLIB_EXPORT_CLASS(velodyne_pointcloud::CloudNodelet, nodelet::Nodelet) diff --git a/velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc b/velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc index fb57b0411..65993be0e 100644 --- a/velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc +++ b/velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc @@ -43,8 +43,7 @@ namespace velodyne_pointcloud } // namespace velodyne_pointcloud -// Register this plugin with pluginlib. Names must match nodelet_velodyne.xml. +// Register this plugin with pluginlib. Names must match nodelets.xml. // -// parameters: package, class name, class type, base class type -PLUGINLIB_DECLARE_CLASS(velodyne_pointcloud, RingColorsNodelet, - velodyne_pointcloud::RingColorsNodelet, nodelet::Nodelet); +// parameters: class type, base class type +PLUGINLIB_EXPORT_CLASS(velodyne_pointcloud::RingColorsNodelet, nodelet::Nodelet) diff --git a/velodyne_pointcloud/src/conversions/transform_nodelet.cc b/velodyne_pointcloud/src/conversions/transform_nodelet.cc index 0154425d2..ef268fbe9 100644 --- a/velodyne_pointcloud/src/conversions/transform_nodelet.cc +++ b/velodyne_pointcloud/src/conversions/transform_nodelet.cc @@ -42,9 +42,7 @@ namespace velodyne_pointcloud } // namespace velodyne_pointcloud -// Register this plugin with pluginlib. Names must match nodelet_velodyne.xml. +// Register this plugin with pluginlib. Names must match nodelets.xml. // -// parameters: package, class name, class type, base class type -PLUGINLIB_DECLARE_CLASS(velodyne_pointcloud, TransformNodelet, - velodyne_pointcloud::TransformNodelet, - nodelet::Nodelet); +// parameters: class type, base class type +PLUGINLIB_EXPORT_CLASS(velodyne_pointcloud::TransformNodelet, nodelet::Nodelet) From 722b8498589fe58b3934474d7a572eb790f7c1f6 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Fri, 10 Nov 2017 14:58:13 -0600 Subject: [PATCH 63/66] Updating change logs for release. --- velodyne/CHANGELOG.rst | 8 +- velodyne_driver/CHANGELOG.rst | 60 ++++++++++--- velodyne_laserscan/CHANGELOG.rst | 16 ++++ velodyne_msgs/CHANGELOG.rst | 4 +- velodyne_pointcloud/CHANGELOG.rst | 145 +++++++++++++++++++++++++----- 5 files changed, 195 insertions(+), 38 deletions(-) create mode 100644 velodyne_laserscan/CHANGELOG.rst diff --git a/velodyne/CHANGELOG.rst b/velodyne/CHANGELOG.rst index 568117341..5d4e05e7c 100644 --- a/velodyne/CHANGELOG.rst +++ b/velodyne/CHANGELOG.rst @@ -1,8 +1,12 @@ Change history ============== -1.3.0 (forthcoming) -------------------- +Forthcoming +------------------ +* Merge pull request `#110 `_ from kmhallen/master + Added velodyne_laserscan package +* Added velodyne_laserscan package and inserted into existing launch files +* Contributors: Jack O'Quin, Joshua Whitley, Kevin Hallenbeck 1.2.0 (2014-08-06) ------------------ diff --git a/velodyne_driver/CHANGELOG.rst b/velodyne_driver/CHANGELOG.rst index 73d22633f..348988327 100644 --- a/velodyne_driver/CHANGELOG.rst +++ b/velodyne_driver/CHANGELOG.rst @@ -1,17 +1,52 @@ Change history ============== -1.3.0 (forthcoming) -------------------- -* Correct VLP-16 packet rate error. -* Use port number when reading PCAP data. -* Fix g++ 5.3.1 compiler errors. -* Add dynamic_reconfigure and time_offset correction. -* Make input destructors virtual. -* Add VLP-16 support. -* Add support for multiple devices. -* Contributors: Brice Rebsamen, ddillenberger, Patrick Hussey, Andreas - Wachaja, priyankadey, Jack O'Quin +Forthcoming +------------------ +* Merge pull request `#129 `_ from kmhallen/pluginlib_macro + Modern pluginlib macro +* Update to use non deprecated pluginlib macro +* add launch args to support multiple devices (`#108 `_) +* Merge pull request `#101 `_ from teosnare/master + velodyne_driver/src/lib/input.cc : fix for device_ip filter +* Merge pull request `#104 `_ from altrouge/launch_options + Add more options in launch files. +* Rearranged alphabetically. +* Add more options in launch files. + - rpm, device_ip, port, read_once, read_fast, repeat_delay +* velodyne_driver/src/lib/input.cc : fix for device_ip filter + Fix for device_ip filter in InputSocket: initialization of devip\_ for correct ip filtering in InputSocket::getPacket. +* velodyne_driver: credit @priyankadey for VLP-16 bug fix (`#96 `_) +* Merge pull request `#96 `_ from priyankadey/master + updated VLP-16 packet rate from user manual. +* updated VLP-16 packet rate from user manual. + Also verified with sensor. It reduced overlap in the pointcloud +* update change history +* Merge pull request `#94 `_ from ros-drivers/pcap_port + velodyne_driver: use port number for PCAP data (`#46 `_, `#66 `_) +* fix g++ 5.3.1 compile errors (`#94 `_) +* merge current master (`#94 `_) +* Merge pull request `#91 `_ from chukcha2/master + update velodyne_driver package description to include all models +* update velodyne_driver package description to include all models +* Merge pull request `#89 `_ from Tones29/feat_dynrec_driver + Add dynamic latency configuration to velodyne_driver +* velodyne_driver: Add dynamic_reconfigure and time_offset correction + The value of time_offset is added to the calculated time stamp in live mode for each packet. +* velodyne_driver: Make input destructors virtual +* prepare change history for coming Indigo release (`#59 `_) +* velodyne_driver: use port number for PCAP data (`#66 `_) +* Merge pull request `#39 `_ from zooxco/multivelodyne + support for multiple velodynes +* Merge pull request `#44 `_ from SISegwayRmp/master + adding driver and pointcloud support for the VLP16 +* adding the VLP16 test scripts and updating the CMakeLists to include the test file from http://download.ros.org/data/velodyne/vlp16.pcap +* adding support for the VLP16 +* parameters to set the udp port +* fixed missing header +* cleanup debug line +* parameter and code added for working with multiple velodynes +* Contributors: Andreas Wachaja, Brice Rebsamen, Daniel Jartoux, Denis Dillenberger, Gabor Meszaros, Ilya, Jack O'Quin, Joshua Whitley, Kevin Hallenbeck, Matteo Murtas, Micho Radovnikovich, Priyanka Dey, William Woodall, jack.oquin, junior, phussey 1.2.0 (2014-08-06) ------------------ @@ -28,8 +63,7 @@ Change history 1.1.1 (2013-07-30) ------------------ - * Add support for HDL-64E S2 and S2.1 models, which were not working - before (`#11`_), thanks to Gabor Meszaros (`#12`_). + * Add support for HDL-64E S2 and S2.1 models, which were not working before (`#11`_), thanks to Gabor Meszaros (`#12`_). * Add additional parameters to launch files (`#14`_). 1.1.0 (2013-07-16) diff --git a/velodyne_laserscan/CHANGELOG.rst b/velodyne_laserscan/CHANGELOG.rst new file mode 100644 index 000000000..7bbe592ba --- /dev/null +++ b/velodyne_laserscan/CHANGELOG.rst @@ -0,0 +1,16 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package velodyne_laserscan +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Merge pull request `#110 `_ from kmhallen/master + Added velodyne_laserscan package +* Added tests for velodyne_laserscan +* Fixed validating PointCloud2 field types +* Package.xml format version 2 +* Merge pull request `#1 `_ from volkandre/master + Fixed bug. Laserscans now cover full 360 degrees. +* Fixed bug. Laserscans now cover full 360 degrees. +* Added velodyne_laserscan package and inserted into existing launch files +* Contributors: Joshua Whitley, Kevin Hallenbeck, kmhallen, volkandre diff --git a/velodyne_msgs/CHANGELOG.rst b/velodyne_msgs/CHANGELOG.rst index b432fc9d8..ea115b649 100644 --- a/velodyne_msgs/CHANGELOG.rst +++ b/velodyne_msgs/CHANGELOG.rst @@ -1,8 +1,8 @@ Change history ============== -1.3.0 (forthcoming) -------------------- +Forthcoming +------------------ 1.2.0 (2014-08-06) ------------------ diff --git a/velodyne_pointcloud/CHANGELOG.rst b/velodyne_pointcloud/CHANGELOG.rst index 4e6b929fe..b7bccd75d 100644 --- a/velodyne_pointcloud/CHANGELOG.rst +++ b/velodyne_pointcloud/CHANGELOG.rst @@ -1,20 +1,131 @@ Change history ============== -1.3.0 (forthcoming) +Forthcoming ------------------- - -* Fix compile warning for "Wrong initialization order". -* Fix unit tests for transform nodelet. -* Provide dynamic reconfiguration for TransformNodelet (`#78`_). -* Allow ``min_range`` as short as 0.4m. -* Add VLP-16 support (`#44`_). -* Fix several errors in point computations (`#43`_, `#47`_, `#50`_, - `#55`_, `#76`_). -* Fix several calibration file problems (`#41`_, `#42`_). -* Contributors: Bo Li, Patrick Hussey, William Woodall, Kun Li, - lemiant, Jose Luis Blanco-Claraco, Alexander Schaefer, François - Pomerleau, Andreas Wachaja, Jack O'Quin +* Merge pull request `#110 `_ from kmhallen/master + Added velodyne_laserscan package +* Merge remote-tracking branch 'ros-drivers/master' +* Merge pull request `#129 `_ from kmhallen/pluginlib_macro + Modern pluginlib macro +* Update to use non deprecated pluginlib macro +* Merge pull request `#127 `_ from swri-robotics/add_vlp16_hires_support + Add VLP16 Puck Hi-Res config file +* Add VLP16 Puck Hi-Res support +* velodyne_pointcloud: remove incorrect catkin_package() DEPENDS option (`#93 `_) + This eliminates a CMake warning when building on Xenial. +* Merge pull request `#111 `_ from OrebroUniversity/master + Added an interface to set up raw data processing offline +* Added an interface to set up raw data processing from a locally defined calibration file. This method is useful when processing data offline from a bag file, without starting any ros master +* Added velodyne_laserscan package and inserted into existing launch files +* test multiple nodelet manager support (`#108 `_) +* add launch args to support multiple devices (`#108 `_) +* Merge pull request `#105 `_ from fudger/patch-1 + Remove unused constants. +* Merge pull request `#104 `_ from altrouge/launch_options + Add more options in launch files. +* Rearranged alphabetically. +* Remove unused constants. + DISTANCE_MAX and DISTANCE_MAX_UNITS are not used anywhere in the code. + Furthermore, using them would lead to errors as both VLP-64 manuals state that returns above 120 m should not be used. The VLP-32 manual allows 70 m as the maximum valid sensor range. +* Merge pull request `#103 `_ from fudger/patch-1 + Fix misleading typecasts. +* Add more options in launch files. + - rpm, device_ip, port, read_once, read_fast, repeat_delay +* Fix misleading typecasts. + intensity and VPoint::intensity are both of type float. +* update change history +* merge current master (`#94 `_) +* Merge pull request `#92 `_ from adasta/master + GCC Build Warnings +* Modified velodyne_point_cloud/src/lib/rawdata.cc to address warning + that last_azimuth_diff variable may be used uninitialized. Variable + is now initialized to 0 at creation. + velodyne/velodyne_pointcloud/src/lib/rawdata.cc:328:57: error: ‘last_azimuth_diff’ may be used uninitialized in this function [-Werror=maybe-uninitialized] + azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION); +* Modified velodyne_pointcloud/src/conversion/colors.cc to remove + address build warning for strict-aliasing. + velodyne/velodyne_pointcloud/src/conversions/colors.cc:84:58: +* Merge pull request `#89 `_ from Tones29/feat_dynrec_driver + Add dynamic latency configuration to velodyne_driver +* velodyne_pointcloud: Fix compile warning "Wrong initialization order" +* velodyne_pointcloud: add dynamic reconfig update to change log (`#78 `_) +* Merge branch 'fudger-reconfigure_transform_node' +* velodyne_pointcloud: use recommended add_dependencies() CMake variable `#78 `_ +* velodyne_pointcloud: fix transform unit tests + Use tf2_ros static_transform_publisher for more consistent timing (`#2 `_) +* Merge branch 'reconfigure_transform_node' of https://github.com/fudger/velodyne +* prepare change history for coming Indigo release (`#59 `_) +* calibration: unit test case improvements (`#84 `_) +* calibration: read all intensities as float, then convert (`#84 `_) +* calibration: add gtest for `#84 `_ + This currently fails on 64e_s2.1-sztaki.yaml and on issue_84_float_intensities.yaml. +* calibration: make max_intensity and min_intensity optional (`#84 `_) + This fixes a regression in the 32e and VLP-16 calibrations which do not contain + intensity values. There is still a problem with the 64e_s2.1 calibration. +* Merge pull request `#76 `_ from pomerlef/master + Sign inversion in some equations +* Merge pull request `#82 `_ from ros-drivers/fix_pr_80 + Fix pr 80; adding travis CI tests. +* fix the yaml-cpp 0.5 code paths +* Merge pull request `#80 `_ from ros-drivers/fix_yaml_import + allow floats in min/max_intensity and make horiz_offset_correction optional +* allow horiz_offset_correction to be optional with 0 as default +* allow floats instead of ints in min/max_intensity +* Resolve frame ID name using tf prefix. +* Improve coding style. +* Set up dynamic reconfiguration for transform_node. + Previously, transform_node has neither read parameters other than frame_id from the command line nor has it exposed these parameters via dynamic reconfigure. As parameters like max_range and view_width have been initialized to zero, the inconfigurable transform_node has returned an empty point cloud. + Now, transform_node launches an reconfigure server just as cloud_node. In contrast to cloud_node, transform node exposes another parameter for dynamic reconfiguration: frame_id, i.e. the frame of reference the incoming Velodyne points are transformed to. +* Merge pull request `#77 `_ from fudger/pretty_print + Fix output of calibration data onto console +* Add a missing space. +* Fix line that always indicates use of model VLP-16. +* Align console output of calibration data. +* Merge branch 'master' of https://github.com/ros-drivers/velodyne +* resolve sign error +* Merge pull request `#73 `_ from fudger/master + Correct important data type error for VLP-16 +* Fix data type error that distorts the point cloud. +* Fix and add a few comments. +* Merge pull request `#68 `_ from jlblancoc/patch-1 + Remove unused variable +* Remove unused variable + I think that `dsr` was unused. See line 317: + for (int dsr=0; ... +* VLP-16: skip badly formatted data packets (`#62 `_, `#63 `_) +* restore VLP-16 min_range setting to 0.4 (`#60 `_) + NOTE: There is still some other problem keeping that from working. +* permit min_range settings below 0.9 meters (`#60 `_) + No known models are currently known to return closer measurements. +* Merge pull request `#55 `_ from lemiant/azimuth_bug_VLP16 + Fixed azimuth overflow bug. +* Fixed azimuth overflow bug. + For interpolated azimuth values between 35999.5 and 36000.0 the nested round(fmod()) + yields a value of 36000 which is invalid and overflows the pre-computed sin/cos arrays, + since they only go form 0..35999 +* Merge pull request `#51 `_ from kunlileo/master + Added vertical sin angle correction +* Added vertical sin angle correction +* Merge pull request `#47 `_ from prclibo/master + fixed rounding bug in intensity calculation found by songshiyu +* fixed rounding bug in intensity calculation found by songshiyu +* fix some overly long C++ source lines +* Merge pull request `#44 `_ from SISegwayRmp/master + adding driver and pointcloud support for the VLP16 +* missed the space in the file name which caused the build to fail, removed space before extension +* adding the VLP16 test scripts and updating the CMakeLists to include the test file from http://download.ros.org/data/velodyne/vlp16.pcap +* adding support for the VLP16 +* Merge pull request `#43 `_ from prclibo/fix_rawdata + fixed point computation according to the 64e_s2(.1) velodyne manual +* fixed point computation according to the 64e_s2(.1) velodyne manual, with luopei"s help +* Merge pull request `#41 `_ from prclibo/master + fixed a calibration file parsing bug +* Merge pull request `#42 `_ from prclibo/fix_gen_calibration + fixed gen_calibration min/max intensity type +* fixed gen_calibration min/max intensity type +* fixed a calibration file parsing bug +* Contributors: Adam Stambler, Alex Rodrigues, Alexander Schaefer, Andreas Wachaja, Bo Li, Daniel Jartoux, Gabor Meszaros, Jack O'Quin, Jose Luis Blanco-Claraco, Joshua Whitley, Kevin Hallenbeck, Kris Kozak, Kun Li, Micho Radovnikovich, Scott K Logan, Thomas Solatges, Todor Stoyanov, William Woodall, jack.oquin, libo24, phussey, piyushk, pomerlef 1.2.0 (2014-08-06) ------------------ @@ -124,12 +235,4 @@ Change history .. _`#17`: https://github.com/ros-drivers/velodyne/issues/17 .. _`#18`: https://github.com/ros-drivers/velodyne/issues/18 .. _`#20`: https://github.com/ros-drivers/velodyne/issues/20 -.. _`#41`: https://github.com/ros-drivers/velodyne/pull/41 -.. _`#42`: https://github.com/ros-drivers/velodyne/pull/42 -.. _`#43`: https://github.com/ros-drivers/velodyne/pull/43 -.. _`#44`: https://github.com/ros-drivers/velodyne/pull/44 -.. _`#47`: https://github.com/ros-drivers/velodyne/pull/47 .. _`#50`: https://github.com/ros-drivers/velodyne/issue/50 -.. _`#55`: https://github.com/ros-drivers/velodyne/pull/55 -.. _`#76`: https://github.com/ros-drivers/velodyne/pull/76 -.. _`#78`: https://github.com/ros-drivers/velodyne/pull/78 From d17e252fc258368575d2363452bf12e5d4c3c7c7 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Fri, 10 Nov 2017 15:05:09 -0600 Subject: [PATCH 64/66] Testing removing single quotes in changelog. --- velodyne_pointcloud/CHANGELOG.rst | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/velodyne_pointcloud/CHANGELOG.rst b/velodyne_pointcloud/CHANGELOG.rst index b7bccd75d..c7b327b1e 100644 --- a/velodyne_pointcloud/CHANGELOG.rst +++ b/velodyne_pointcloud/CHANGELOG.rst @@ -5,7 +5,7 @@ Forthcoming ------------------- * Merge pull request `#110 `_ from kmhallen/master Added velodyne_laserscan package -* Merge remote-tracking branch 'ros-drivers/master' +* Merge remote-tracking branch ros-drivers/master * Merge pull request `#129 `_ from kmhallen/pluginlib_macro Modern pluginlib macro * Update to use non deprecated pluginlib macro @@ -50,11 +50,11 @@ Forthcoming Add dynamic latency configuration to velodyne_driver * velodyne_pointcloud: Fix compile warning "Wrong initialization order" * velodyne_pointcloud: add dynamic reconfig update to change log (`#78 `_) -* Merge branch 'fudger-reconfigure_transform_node' +* Merge branch fudger-reconfigure_transform_node * velodyne_pointcloud: use recommended add_dependencies() CMake variable `#78 `_ * velodyne_pointcloud: fix transform unit tests Use tf2_ros static_transform_publisher for more consistent timing (`#2 `_) -* Merge branch 'reconfigure_transform_node' of https://github.com/fudger/velodyne +* Merge branch reconfigure_transform_node of https://github.com/fudger/velodyne * prepare change history for coming Indigo release (`#59 `_) * calibration: unit test case improvements (`#84 `_) * calibration: read all intensities as float, then convert (`#84 `_) @@ -82,7 +82,7 @@ Forthcoming * Add a missing space. * Fix line that always indicates use of model VLP-16. * Align console output of calibration data. -* Merge branch 'master' of https://github.com/ros-drivers/velodyne +* Merge branch master of https://github.com/ros-drivers/velodyne * resolve sign error * Merge pull request `#73 `_ from fudger/master Correct important data type error for VLP-16 @@ -125,7 +125,7 @@ Forthcoming fixed gen_calibration min/max intensity type * fixed gen_calibration min/max intensity type * fixed a calibration file parsing bug -* Contributors: Adam Stambler, Alex Rodrigues, Alexander Schaefer, Andreas Wachaja, Bo Li, Daniel Jartoux, Gabor Meszaros, Jack O'Quin, Jose Luis Blanco-Claraco, Joshua Whitley, Kevin Hallenbeck, Kris Kozak, Kun Li, Micho Radovnikovich, Scott K Logan, Thomas Solatges, Todor Stoyanov, William Woodall, jack.oquin, libo24, phussey, piyushk, pomerlef +* Contributors: Adam Stambler, Alex Rodrigues, Alexander Schaefer, Andreas Wachaja, Bo Li, Daniel Jartoux, Gabor Meszaros, Jack OQuin, Jose Luis Blanco-Claraco, Joshua Whitley, Kevin Hallenbeck, Kris Kozak, Kun Li, Micho Radovnikovich, Scott K Logan, Thomas Solatges, Todor Stoyanov, William Woodall, jack.oquin, libo24, phussey, piyushk, pomerlef 1.2.0 (2014-08-06) ------------------ @@ -140,7 +140,7 @@ Forthcoming * Add dynamic_reconfigure feature. * Add angular limits to the output point cloud, useful for omitting part of it. (`#22 `_). -* Contributors: Jack O'Quin, Scott K Logan, Thomas Solatges +* Contributors: Jack OQuin, Scott K Logan, Thomas Solatges 1.1.2 (2013-11-05) ------------------ @@ -155,7 +155,7 @@ Forthcoming * Add support for HDL-64E S2 and S2.1 models, which were not working before (`#11`_), thanks to Gabor Meszaros (`#12`_). * Add additional parameters to launch files (`#14`_). - * Contributors: Gabor Meszaros, Jack O'Quin + * Contributors: Gabor Meszaros, Jack OQuin 1.1.0 (2013-07-16) ------------------ From f8c6e8244e60a25b09ec6f2d1debc64c30e9852a Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Fri, 10 Nov 2017 15:10:12 -0600 Subject: [PATCH 65/66] Bumping version. --- velodyne/CHANGELOG.rst | 2 +- velodyne/package.xml | 4 ++-- velodyne_driver/CHANGELOG.rst | 2 +- velodyne_driver/package.xml | 4 ++-- velodyne_laserscan/CHANGELOG.rst | 4 ++-- velodyne_laserscan/package.xml | 4 ++-- velodyne_msgs/CHANGELOG.rst | 2 +- velodyne_msgs/package.xml | 4 ++-- velodyne_pointcloud/CHANGELOG.rst | 2 +- velodyne_pointcloud/package.xml | 4 ++-- 10 files changed, 16 insertions(+), 16 deletions(-) diff --git a/velodyne/CHANGELOG.rst b/velodyne/CHANGELOG.rst index 5d4e05e7c..94c63f794 100644 --- a/velodyne/CHANGELOG.rst +++ b/velodyne/CHANGELOG.rst @@ -1,7 +1,7 @@ Change history ============== -Forthcoming +1.3.0 (2017-11-10) ------------------ * Merge pull request `#110 `_ from kmhallen/master Added velodyne_laserscan package diff --git a/velodyne/package.xml b/velodyne/package.xml index 2688b1f08..d1ddfe34a 100644 --- a/velodyne/package.xml +++ b/velodyne/package.xml @@ -1,11 +1,11 @@ velodyne - 1.2.0 + 1.3.0 Basic ROS support for the Velodyne 3D LIDARs. - Jack O'Quin + Josh Whitley Jack O'Quin BSD