Initial commit

sensors/velodyne/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})
+

sensors/velodyne/velodyne_laserscan/tests/lazy_subscriber.cpp

@@ -0,0 +1,124 @@
+// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+// 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<sensor_msgs::PointCloud2>("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();
+}

sensors/velodyne/velodyne_laserscan/tests/lazy_subscriber_node.test

@@ -0,0 +1,17 @@
+<!-- -*- mode: XML -*- -->
+<!-- rostest of the lazy subscriber -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan node -->
+  <node pkg="velodyne_laserscan" type="velodyne_laserscan_node" name="laserscan" output="$(arg output)" />
+
+  <!-- Start the rostest -->
+  <test test-name="test_lazy_subscriber_node" pkg="velodyne_laserscan"
+        type="test_lazy_subscriber_node" name="test_lazy_subscriber">
+  </test>
+
+</launch>

sensors/velodyne/velodyne_laserscan/tests/lazy_subscriber_nodelet.test

@@ -0,0 +1,21 @@
+<!-- -*- mode: XML -*- -->
+<!-- rostest of the lazy subscriber -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan nodelet -->
+  <arg name="manager" default="nodelet_manager" />
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager"  output="$(arg output)" />
+  <node pkg="nodelet" type="nodelet" name="velodyne_laserscan"  output="$(arg output)"
+        args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
+  </node>
+
+  <!-- Start the rostest -->
+  <test test-name="test_lazy_subscriber_nodelet" pkg="velodyne_laserscan"
+        type="test_lazy_subscriber_nodelet" name="test_lazy_subscriber">
+  </test>
+
+</launch>

sensors/velodyne/velodyne_laserscan/tests/system.cpp

@@ -0,0 +1,711 @@
+// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <cstdlib>
+#include <algorithm>
+#include <vector>
+
+// 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<Point> 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++)
+  {
+    *(reinterpret_cast<float*>(ptr +  0)) = cloud.points[i].x;
+    *(reinterpret_cast<float*>(ptr +  4)) = cloud.points[i].y;
+    *(reinterpret_cast<float*>(ptr +  8)) = cloud.points[i].z;
+    *(reinterpret_cast<float*>(ptr + 16)) = cloud.points[i].i;
+    *(reinterpret_cast<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++)
+  {
+    *(reinterpret_cast<float*>(ptr +  0)) = cloud.points[i].i;
+    *(reinterpret_cast<float*>(ptr +  4)) = cloud.points[i].z;
+    *(reinterpret_cast<float*>(ptr +  8)) = cloud.points[i].y;
+    *(reinterpret_cast<float*>(ptr + 12)) = cloud.points[i].x;
+    *(reinterpret_cast<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++)
+  {
+    *(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
+    *(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
+    *(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
+    *(reinterpret_cast<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++)
+  {
+    *(reinterpret_cast<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++)
+  {
+    *(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
+    *(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
+    *(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
+    *(reinterpret_cast<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++)
+  {
+    *(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
+    *(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
+    *(reinterpret_cast<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 + static_cast<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 + static_cast<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 = std::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 = std::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 = std::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 = std::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<sensor_msgs::PointCloud2>("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();
+}

sensors/velodyne/velodyne_laserscan/tests/system_node.test

@@ -0,0 +1,17 @@
+<!-- -*- mode: XML -*- -->
+<!-- rostest of the velodyne_laserscan system -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan node -->
+  <node pkg="velodyne_laserscan" type="velodyne_laserscan_node" name="laserscan" output="$(arg output)" />
+
+  <!-- Start the rostest -->
+  <test test-name="test_system_node" pkg="velodyne_laserscan"
+        type="test_system_node" name="test_system">
+  </test>
+
+</launch>

sensors/velodyne/velodyne_laserscan/tests/system_nodelet.test

@@ -0,0 +1,21 @@
+<!-- -*- mode: XML -*- -->
+<!-- rostest of the velodyne_laserscan system -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan nodelet -->
+  <arg name="manager" default="nodelet_manager" />
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="$(arg output)" />
+  <node pkg="nodelet" type="nodelet" name="velodyne_laserscan" output="$(arg output)"
+        args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
+  </node>
+
+  <!-- Start the rostest -->
+  <test test-name="test_system_nodelet" pkg="velodyne_laserscan"
+        type="test_system_nodelet" name="test_system">
+  </test>
+
+</launch>