Added tests for velodyne_laserscan

velodyne_laserscan/CMakeLists.txt

@@ -44,3 +44,8 @@ install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
 install(FILES nodelets.xml
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+if (CATKIN_ENABLE_TESTING)
+  add_subdirectory(tests)
+endif()
+

velodyne_laserscan/package.xml

@@ -21,6 +21,9 @@
   <depend>sensor_msgs</depend>
   <depend>dynamic_reconfigure</depend>
 
+  <test_depend>roslaunch</test_depend>
+  <test_depend>rostest</test_depend>
+
   <export>
     <nodelet plugin="${prefix}/nodelets.xml" />
   </export>

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})
+

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 <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();
+}

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>

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>