1+ #include < helpers/commonHelpers.hpp>
2+ #include < helpers/graphHelpers.hpp>
3+ #include < helpers/testPointCloud.hpp>
4+
5+ #include < rgl/api/extensions/ros2.h>
6+
7+ #include < rclcpp/rclcpp.hpp>
8+ #include < sensor_msgs/msg/laser_scan.hpp>
9+
10+ class Ros2PublishLaserScanNodeTest : public RGLTest
11+ {};
12+
13+ TEST_F (Ros2PublishLaserScanNodeTest, should_throw_invalid_pipeline_when_ros2_shutdown_laserscan)
14+ {
15+ std::vector<rgl_field_t > fields{AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32};
16+ TestPointCloud pointCloud (fields, 10 );
17+ rgl_node_t points = pointCloud.createUsePointsNode ();
18+
19+ rgl_node_t yield = nullptr ;
20+ EXPECT_RGL_SUCCESS (rgl_node_points_yield (&yield, fields.data (), fields.size ()));
21+
22+ rgl_node_t ros2pub = nullptr ;
23+
24+ EXPECT_RGL_SUCCESS (rgl_node_publish_ros2_laserscan (&ros2pub, " laserscan" " rglFrame"
25+
26+ EXPECT_RGL_SUCCESS (rgl_graph_node_add_child (points, yield));
27+ EXPECT_RGL_SUCCESS (rgl_graph_node_add_child (yield, ros2pub));
28+
29+ EXPECT_RGL_SUCCESS (rgl_graph_run (points));
30+
31+ rclcpp::shutdown ();
32+ EXPECT_RGL_INVALID_PIPELINE (rgl_graph_run (points), " Unable to execute Ros2Node because ROS2 has been shut down."
33+ }
34+
35+ TEST_F (Ros2PublishLaserScanNodeTest, should_receive_sent_data)
36+ {
37+ const auto POINT_COUNT = 5 ;
38+ const auto TOPIC_NAME = " rgl_test_laser_scan"
39+ const auto FRAME_ID = " rgl_test_frame_id"
40+ const auto NODE_NAME = " rgl_test_node"
41+ const auto WAIT_TIME_SECS = 1 ;
42+ const auto MESSAGE_REPEATS = 3 ;
43+ const auto START_ANGLE = 0 .8f ;
44+ const auto ANGLE_INCREMENT = 0 .11f ;
45+ const auto TIME_INCREMENT = 0 .22f ;
46+
47+ TestPointCloud input ({AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32}, POINT_COUNT);
48+
49+ // Expected data
50+ const auto expectedAngles = generateFieldValues (
51+ POINT_COUNT, std::function<Field<AZIMUTH_F32>::type (int )>(
52+ [START_ANGLE, ANGLE_INCREMENT](int i) { return START_ANGLE + i * ANGLE_INCREMENT; }));
53+ input.
setFieldValues <AZIMUTH_F32>(expectedAngles);
54+ const auto expectedTimes = generateFieldValues (POINT_COUNT, std::function<Field<TIME_STAMP_F64>::type (int )>(
55+ [TIME_INCREMENT](int i) { return i * TIME_INCREMENT; }));
56+ input.setFieldValues <TIME_STAMP_F64>(expectedTimes);
57+ const auto expectedRanges = input.getFieldValues <DISTANCE_F32>();
58+ const auto expectedIntensities = input.getFieldValues <INTENSITY_F32>();
59+ const auto [minRange, maxRange] = std::minmax_element (expectedRanges.begin (), expectedRanges.end ());
60+ const auto expectedScanTime = expectedTimes.at (expectedTimes.size () - 1 ) - expectedTimes.at (0 );
61+
62+ // Create nodes
63+ rgl_node_t inputNode = input.createUsePointsNode (), laserScanNode = nullptr ;
64+ ASSERT_RGL_SUCCESS (rgl_node_publish_ros2_laserscan (&laserScanNode, TOPIC_NAME, FRAME_ID));
65+
66+ // Connect nodes
67+ ASSERT_RGL_SUCCESS (rgl_graph_node_add_child (inputNode, laserScanNode));
68+
69+ // Synchronization primitives
70+ std::atomic<int > messageCount = 0 ;
71+
72+
73+ // Create ROS2 subscriber to receive LaserScan messages on topic "laser_scan"
74+ auto node = std::make_shared<rclcpp::Node>(NODE_NAME, rclcpp::NodeOptions{});
75+ auto qos = rclcpp::QoS (10 );
76+ qos.reliability (static_cast <rmw_qos_reliability_policy_t >(QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT));
77+ qos.durability (static_cast <rmw_qos_durability_policy_t >(QOS_POLICY_DURABILITY_SYSTEM_DEFAULT));
78+ qos.history (static_cast <rmw_qos_history_policy_t >(QOS_POLICY_HISTORY_SYSTEM_DEFAULT));
79+ auto subscriber = node->create_subscription <sensor_msgs::msg::LaserScan>(
80+ TOPIC_NAME, qos, [&](const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) {
81+ EXPECT_EQ (msg->header .frame_id , FRAME_ID);
82+ EXPECT_NE (msg->header .stamp .sec + msg->header .stamp .nanosec , 0 );
83+
84+ EXPECT_NEAR (msg->angle_min , START_ANGLE, EPSILON_F);
85+ EXPECT_NEAR (msg->angle_max , START_ANGLE + (POINT_COUNT - 1 ) * ANGLE_INCREMENT, EPSILON_F);
86+ EXPECT_NEAR (msg->range_min , *minRange, EPSILON_F);
87+ EXPECT_NEAR (msg->range_max , *maxRange, EPSILON_F);
88+ EXPECT_NEAR (msg->angle_increment , ANGLE_INCREMENT, EPSILON_F);
89+ EXPECT_NEAR (msg->time_increment , TIME_INCREMENT, EPSILON_F);
90+ EXPECT_NEAR (msg->scan_time , expectedScanTime, EPSILON_F);
91+
92+ ASSERT_EQ (msg->ranges .size (), POINT_COUNT);
93+ ASSERT_EQ (msg->intensities .size (), POINT_COUNT);
94+ for (int i = 0 ; i < POINT_COUNT; ++i) {
95+ EXPECT_NEAR (msg->ranges [i], expectedRanges[i], EPSILON_F);
96+ EXPECT_NEAR (msg->intensities [i], expectedIntensities[i], EPSILON_F);
97+ }
98+ ++messageCount;
99+ });
100+
101+ // Run
102+ for (int i = 0 ; i < MESSAGE_REPEATS; ++i) {
103+ ASSERT_RGL_SUCCESS (rgl_graph_run (inputNode));
104+ }
105+
106+ // Wait for messages
107+ {
108+ auto start = std::chrono::steady_clock::now ();
109+ do {
110+ rclcpp::spin_some (node);
111+ std::this_thread::sleep_for (std::chrono::milliseconds (10 ));
112+ } while (messageCount != MESSAGE_REPEATS &&
113+ std::chrono::steady_clock::now () - start < std::chrono::seconds (WAIT_TIME_SECS));
114+ ASSERT_EQ (messageCount, MESSAGE_REPEATS);
115+ }
116+ }
0 commit comments