diff --git a/.gitignore b/.gitignore index d53a461ce..da9cbf726 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,6 @@ *.pyc *.tar.gz *~ +*.user +doc TAGS diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 000000000..648538765 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,33 @@ +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, 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_isolated diff --git a/README.rst b/README.rst index 88e71fe05..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 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 diff --git a/velodyne/CHANGELOG.rst b/velodyne/CHANGELOG.rst index 6eea4b978..94c63f794 100644 --- a/velodyne/CHANGELOG.rst +++ b/velodyne/CHANGELOG.rst @@ -1,6 +1,13 @@ Change history ============== +1.3.0 (2017-11-10) +------------------ +* 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/package.xml b/velodyne/package.xml index 89511dd56..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 - + - - + + + - + + - - + + + + + + + + + - + + - + diff --git a/velodyne_driver/package.xml b/velodyne_driver/package.xml index c8be55d26..3e36b82e4 100644 --- a/velodyne_driver/package.xml +++ b/velodyne_driver/package.xml @@ -1,11 +1,12 @@ velodyne_driver - 1.2.0 + 1.3.0 - ROS device driver for Velodyne HDL-64E, and HDL-32 LIDARs. + ROS device driver for Velodyne 3D LIDARs. - Jack O'Quin + Josh Whitley + Brice Rebsamen Jack O'Quin Patrick Beeson Michael Quinlan @@ -19,6 +20,7 @@ catkin diagnostic_updater + dynamic_reconfigure libpcap nodelet pluginlib @@ -31,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 729cffb4e..a80c6a8f2 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 @@ -82,6 +82,17 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node, private_nh.param("cut_angle", cut_angle, -0.01); config_.cut_angle = int(cut_angle*100); + int udp_port; + private_nh.param("port", udp_port, (int) DATA_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; @@ -97,19 +108,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 { - input_.reset(new velodyne_driver::InputSocket(private_nh)); + // read data from live socket + input_.reset(new velodyne_driver::InputSocket(private_nh, udp_port)); } - // raw data output topic - output_ = node.advertise("velodyne_packets", 10); + // raw packet output topic + output_ = + node.advertise("velodyne_packets", 10); } /** poll the device @@ -129,7 +142,7 @@ bool VelodyneDriver::poll(void) { while(true) { - int rc = input_->getPacket(&tmp_packet); + int rc = input_->getPacket(&tmp_packet, config_.time_offset); if (rc == 0) break; // got a full packet? if (rc < 0) return false; // end of file reached? } @@ -163,7 +176,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? } @@ -172,7 +185,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.back().stamp; scan->header.frame_id = config_.frame_id; output_.publish(scan); @@ -184,4 +197,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 e8225edd4..68b58e180 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 { @@ -45,6 +55,7 @@ class VelodyneDriver int npackets; ///< number of packets to collect double rpm; ///< device rotation rate (RPMs) int cut_angle; ///< cutting angle in 1/100° + double time_offset; ///< time in seconds added to each velodyne time stamp } config_; boost::shared_ptr input_; 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_driver/src/lib/input.cc b/velodyne_driver/src/lib/input.cc index 8f7d22493..382b0bafe 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, uint16_t 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,20 @@ 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, uint16_t port): + 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 " << udp_port); + ROS_INFO_STREAM("Opening UDP socket: port " << port); sockfd_ = socket(PF_INET, SOCK_DGRAM, 0); if (sockfd_ == -1) { @@ -60,7 +87,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) @@ -85,7 +112,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(); @@ -94,6 +121,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,21 +170,29 @@ 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) - { + 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, done now - break; + // 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) + continue; + else + break; //done } ROS_DEBUG_STREAM("incomplete Velodyne packet read: " @@ -162,9 +200,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; } @@ -175,31 +213,25 @@ 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, 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) { - 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."); @@ -216,8 +248,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) @@ -225,9 +265,8 @@ namespace velodyne_driver pcap_close(pcap_); } - /** @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; @@ -237,12 +276,19 @@ namespace velodyne_driver int res; if ((res = pcap_next_ex(pcap_, &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. if (read_fast_ == false) 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 } diff --git a/velodyne_laserscan/CHANGELOG.rst b/velodyne_laserscan/CHANGELOG.rst new file mode 100644 index 000000000..7b0f22222 --- /dev/null +++ b/velodyne_laserscan/CHANGELOG.rst @@ -0,0 +1,16 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package velodyne_laserscan +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.3.0 (2017-11-10) +------------------ +* 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_laserscan/CMakeLists.txt b/velodyne_laserscan/CMakeLists.txt new file mode 100644 index 000000000..09e92b993 --- /dev/null +++ b/velodyne_laserscan/CMakeLists.txt @@ -0,0 +1,51 @@ +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} +) + +if (CATKIN_ENABLE_TESTING) + add_subdirectory(tests) +endif() + 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..c2801643f --- /dev/null +++ b/velodyne_laserscan/package.xml @@ -0,0 +1,31 @@ + + + velodyne_laserscan + 1.3.0 + + Extract a single ring of a Velodyne PointCloud2 and publish it as a LaserScan message + + Josh Whitley + 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 + + roslaunch + rostest + + + + + + diff --git a/velodyne_laserscan/src/VelodyneLaserScan.cpp b/velodyne_laserscan/src/VelodyneLaserScan.cpp new file mode 100644 index 000000000..85990125b --- /dev/null +++ b/velodyne_laserscan/src/VelodyneLaserScan.cpp @@ -0,0 +1,174 @@ +#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_) { + // 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) { + 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: Field 'ring' of type 'UINT16' 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].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; + } + } + } + + // Construct LaserScan message + if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0)) { + const float RESOLUTION = fabsf(cfg_.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; + 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_r(*msg, "ring"); + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_i(*msg, "intensity"); + 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 + 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_r(*msg, "ring"); + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + 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 + 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_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 @@ + + + + + + + + + + + + + + + + + + + diff --git a/velodyne_msgs/CHANGELOG.rst b/velodyne_msgs/CHANGELOG.rst index b2f5ff838..d4824cda0 100644 --- a/velodyne_msgs/CHANGELOG.rst +++ b/velodyne_msgs/CHANGELOG.rst @@ -1,6 +1,9 @@ Change history ============== +1.3.0 (2017-11-10) +------------------ + 1.2.0 (2014-08-06) ------------------ diff --git a/velodyne_msgs/package.xml b/velodyne_msgs/package.xml index 0091290f1..1ac30eeb6 100644 --- a/velodyne_msgs/package.xml +++ b/velodyne_msgs/package.xml @@ -1,11 +1,11 @@ velodyne_msgs - 1.2.0 + 1.3.0 ROS message definitions for Velodyne 3D LIDARs. - Jack O'Quin + Josh Whitley Jack O'Quin BSD diff --git a/velodyne_pointcloud/CHANGELOG.rst b/velodyne_pointcloud/CHANGELOG.rst index 2c5ea9cf0..064fc43a7 100644 --- a/velodyne_pointcloud/CHANGELOG.rst +++ b/velodyne_pointcloud/CHANGELOG.rst @@ -1,8 +1,135 @@ Change history ============== +1.3.0 (2017-11-10) +------------------- +* 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 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) ------------------ + * velodyne_pointcloud: remove model-dependent "constants" from rawdata.h (`#28 `_) @@ -13,10 +140,10 @@ Change history * 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) -------------------- +------------------ * Move unit test data to download.ros.org (`#18`_). * Install missing gen_calibration.py script (`#20`_). @@ -28,7 +155,7 @@ Change history * 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) ------------------ @@ -108,3 +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 +.. _`#50`: https://github.com/ros-drivers/velodyne/issue/50 diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index cc3883196..cefd96d72 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) @@ -26,19 +28,23 @@ 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) + 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} - DEPENDS yaml-cpp python-yaml INCLUDE_DIRS include LIBRARIES velodyne_rawdata) @@ -63,50 +69,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/cfg/VelodyneConfig.cfg b/velodyne_pointcloud/cfg/CloudNode.cfg similarity index 84% rename from velodyne_pointcloud/cfg/VelodyneConfig.cfg rename to velodyne_pointcloud/cfg/CloudNode.cfg index 5fa57e9b5..c4f45c783 100755 --- a/velodyne_pointcloud/cfg/VelodyneConfig.cfg +++ b/velodyne_pointcloud/cfg/CloudNode.cfg @@ -6,11 +6,11 @@ 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", 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..e6948b9b7 --- /dev/null +++ b/velodyne_pointcloud/cfg/TransformNode.cfg @@ -0,0 +1,39 @@ +#!/usr/bin/env python +PACKAGE = "velodyne_pointcloud" + +from math import pi +import dynamic_reconfigure.parameter_generator_catkin as pgc + +gen = pgc.ParameterGenerator() + +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")) 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/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index b4fb077b6..02fa13c8e 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h @@ -46,25 +46,21 @@ 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; /** 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. @@ -137,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/launch/32e_points.launch b/velodyne_pointcloud/launch/32e_points.launch index 7a5eb1165..8e25c9e25 100644 --- a/velodyne_pointcloud/launch/32e_points.launch +++ b/velodyne_pointcloud/launch/32e_points.launch @@ -1,27 +1,53 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index 251fb86b9..7a18e33d6 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -1,30 +1,53 @@ - + - - + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + diff --git a/velodyne_pointcloud/launch/cloud_nodelet.launch b/velodyne_pointcloud/launch/cloud_nodelet.launch index a39bf80cb..049a89f51 100644 --- a/velodyne_pointcloud/launch/cloud_nodelet.launch +++ b/velodyne_pointcloud/launch/cloud_nodelet.launch @@ -1,19 +1,16 @@ - + - + - + + + - + 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/launch/transform_nodelet.launch b/velodyne_pointcloud/launch/transform_nodelet.launch index ede8e376e..465f33fc2 100644 --- a/velodyne_pointcloud/launch/transform_nodelet.launch +++ b/velodyne_pointcloud/launch/transform_nodelet.launch @@ -1,21 +1,17 @@ - + - - - + + + + - - + + diff --git a/velodyne_pointcloud/package.xml b/velodyne_pointcloud/package.xml index 70d95f820..c6d28e9de 100644 --- a/velodyne_pointcloud/package.xml +++ b/velodyne_pointcloud/package.xml @@ -1,11 +1,11 @@ velodyne_pointcloud - 1.2.0 + 1.3.0 Point cloud conversions for Velodyne 3D LIDARs. - Jack O'Quin + Josh Whitley Jack O'Quin Piyush Khandelwal Jesse Vera @@ -33,6 +33,7 @@ roslaunch rostest + tf2_ros angles nodelet @@ -47,6 +48,7 @@ velodyne_msgs yaml-cpp dynamic_reconfigure + velodyne_laserscan 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 diff --git a/velodyne_pointcloud/src/conversions/CMakeLists.txt b/velodyne_pointcloud/src/conversions/CMakeLists.txt index 15568e3de..8e6e25196 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}_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}_EXPORTED_TARGETS}) 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}_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}_EXPORTED_TARGETS}) target_link_libraries(transform_nodelet velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS transform_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/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; 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/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.cc b/velodyne_pointcloud/src/conversions/transform.cc index a936086d7..bce8b035e 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -25,19 +25,23 @@ namespace velodyne_pointcloud { /** @brief Constructor. */ Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh): + tf_prefix_(tf::getPrefixParam(private_nh)), data_(new velodyne_rawdata::RawData()) { - 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) output_ = node.advertise("velodyne_points", 10); + srv_ = boost::make_shared > (private_nh); + dynamic_reconfigure::Server:: + CallbackType f; + f = boost::bind (&Transform::reconfigure_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 +50,16 @@ namespace velodyne_pointcloud config_.frame_id, 10); tf_filter_->registerCallback(boost::bind(&Transform::processScan, this, _1)); } + + void Transform::reconfigure_callback( + velodyne_pointcloud::TransformNodeConfig &config, uint32_t level) + { + ROS_INFO_STREAM("Reconfigure request."); + data_->setParameters(config.min_range, config.max_range, + config.view_direction, config.view_width); + 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. * @@ -90,7 +104,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_); diff --git a/velodyne_pointcloud/src/conversions/transform.h b/velodyne_pointcloud/src/conversions/transform.h index 734f1d49a..3ee0c3f7d 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,13 @@ namespace velodyne_pointcloud void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg); + ///Pointer to dynamic reconfigure service srv_ + boost::shared_ptr > srv_; + 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_; 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) diff --git a/velodyne_pointcloud/src/lib/calibration.cc b/velodyne_pointcloud/src/lib/calibration.cc index e40c828f6..c470c5918 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) { @@ -73,25 +74,54 @@ 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[MAX_INTENSITY]) - node[MAX_INTENSITY] >> correction.second.max_intensity; + if (node[HORIZ_OFFSET_CORRECTION]) + node[HORIZ_OFFSET_CORRECTION] >> + correction.second.horiz_offset_correction; #else - if (const YAML::Node *pName = node.FindValue(MAX_INTENSITY)) - *pName >> correction.second.max_intensity; + 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 = NULL; +#ifdef HAVE_NEW_YAMLCPP + if (node[MAX_INTENSITY]) { + const YAML::Node max_intensity_node_ref = node[MAX_INTENSITY]; + max_intensity_node = &max_intensity_node_ref; + } +#else + if (const YAML::Node *pName = node.FindValue(MAX_INTENSITY)) + max_intensity_node = pName; +#endif + if (max_intensity_node) { + float max_intensity_float; + *max_intensity_node >> max_intensity_float; + correction.second.max_intensity = floor(max_intensity_float); + } + else { correction.second.max_intensity = 255; + } + + const YAML::Node * min_intensity_node = NULL; #ifdef HAVE_NEW_YAMLCPP - if (node[MIN_INTENSITY]) - node[MIN_INTENSITY] >> correction.second.min_intensity; + if (node[MIN_INTENSITY]) { + const YAML::Node min_intensity_node_ref = node[MIN_INTENSITY]; + min_intensity_node = &min_intensity_node_ref; + } #else if (const YAML::Node *pName = node.FindValue(MIN_INTENSITY)) - *pName >> correction.second.min_intensity; + min_intensity_node = pName; #endif - else + if (min_intensity_node) { + float min_intensity_float; + *min_intensity_node >> min_intensity_float; + correction.second.min_intensity = floor(min_intensity_float); + } + else { correction.second.min_intensity = 0; + } node[FOCAL_DISTANCE] >> correction.second.focal_distance; node[FOCAL_SLOPE] >> correction.second.focal_slope; @@ -108,6 +138,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; @@ -145,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_STREAM("laser_ring[" << next_index << "] = " << ring - << ", angle = " << next_angle); + if (calibration.ros_info) { + ROS_INFO("laser_ring[%2u] = %2u, angle = %+.6f", + next_index, ring, next_angle); + } } } } diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index ec6fc6aaf..77f370718 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, @@ -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) { @@ -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 @@ -183,7 +215,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 +247,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. @@ -264,7 +296,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); @@ -285,17 +317,27 @@ 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; 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++) { - 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 + } + + // 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); @@ -326,6 +368,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; @@ -351,7 +395,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; @@ -383,7 +427,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; @@ -391,7 +435,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 @@ -409,7 +453,6 @@ namespace velodyne_rawdata float z_coord = z; /** Intensity Calculation */ - float min_intensity = corrections.min_intensity; float max_intensity = corrections.max_intensity; @@ -426,15 +469,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 + point.intensity = intensity; + pc.points.push_back(point); ++pc.width; } diff --git a/velodyne_pointcloud/tests/CMakeLists.txt b/velodyne_pointcloud/tests/CMakeLists.txt new file mode 100644 index 000000000..a8f7ac14e --- /dev/null +++ b/velodyne_pointcloud/tests/CMakeLists.txt @@ -0,0 +1,52 @@ +### 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) +find_package(tf2_ros 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) +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/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 @@ - - - + + + - - - - - 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/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" /> diff --git a/velodyne_pointcloud/tests/test_calibration.cpp b/velodyne_pointcloud/tests/test_calibration.cpp new file mode 100644 index 000000000..5139eca04 --- /dev/null +++ b/velodyne_pointcloud/tests/test_calibration.cpp @@ -0,0 +1,164 @@ +// +// 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(false); + calibration.read("./no_such_file.yaml"); + EXPECT_FALSE(calibration.initialized); +} + +TEST(Calibration, vlp16) +{ + 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", 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", 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", + 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", + 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() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + init_global_data(); + return RUN_ALL_TESTS(); +} + 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +