Initial commit

2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
@@ -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();
+}