From b3978bcca9ccb457ef3a47f2199a31715f509980 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Fri, 31 May 2024 18:08:40 +0300 Subject: [PATCH] heartbeat tests pass MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .../src/control_center_node.cpp | 1 + .../test/test_cc_heartbeat.cpp | 27 ++++++---- .../test/test_cc_registering.cpp | 14 ++--- .../test/test_utility.hpp | 53 ++++++++++++------- 4 files changed, 58 insertions(+), 37 deletions(-) diff --git a/common/autoware_control_center/src/control_center_node.cpp b/common/autoware_control_center/src/control_center_node.cpp index 39d9c41f..1e4ad93b 100644 --- a/common/autoware_control_center/src/control_center_node.cpp +++ b/common/autoware_control_center/src/control_center_node.cpp @@ -166,6 +166,7 @@ void ControlCenter::on_heartbeat( node_info->report.stamp_last_heartbeat = heartbeat->stamp; node_info->report.status_activity = heartbeat->status_activity; node_info->report.status_operational = heartbeat->status_operational; + node_info->report.is_alive = true; } void ControlCenter::on_deadline_missed( diff --git a/common/autoware_control_center/test/test_cc_heartbeat.cpp b/common/autoware_control_center/test/test_cc_heartbeat.cpp index 03a64bb7..49e74a8b 100644 --- a/common/autoware_control_center/test/test_cc_heartbeat.cpp +++ b/common/autoware_control_center/test/test_cc_heartbeat.cpp @@ -64,7 +64,7 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatHandling) RCLCPP_INFO(rclcpp::get_logger("test"), "Test started"); const std::string node_name = "node_test_heartbeat"; const std::string ns; - const auto [uuid, node_fully_qualified_name] = test::register_node(node_name, ns); + const auto [uuid, node] = test::register_node(node_name, ns); // Send a heartbeat and verify node status is updated autoware_control_center_msgs::msg::NodeStatusActivity status_activity; @@ -73,9 +73,9 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatHandling) status_operational.status = autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL; RCLCPP_INFO(rclcpp::get_logger("test"), "Sending heartbeat"); - test::send_heartbeat(node_name, ns, deadline_ms_, status_activity, status_operational); + auto pub_hb = test::send_first_heartbeat(node, deadline_ms_, status_activity, status_operational); RCLCPP_INFO(rclcpp::get_logger("test"), "Sent heartbeat"); - test::send_heartbeat(node_name, ns, deadline_ms_, status_activity, status_operational); + test::send_heartbeat(node, pub_hb, status_activity, status_operational); RCLCPP_INFO(rclcpp::get_logger("test"), "Sent 2nd heartbeat"); // wait for the heartbeat to be processed @@ -83,7 +83,7 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatHandling) RCLCPP_INFO(rclcpp::get_logger("test"), "Slept a bit"); autoware_control_center_msgs::msg::NodeReport report; - ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report)); + ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report)); ASSERT_EQ(report.status_activity.status, status_activity.status); ASSERT_EQ(report.status_operational.status, status_operational.status); } @@ -92,14 +92,22 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatMissedDeadline) { const std::string node_name = "node_test_heartbeat"; const std::string ns; - const auto [uuid, node_fully_qualified_name] = test::register_node(node_name, ns); + const auto [uuid, node] = test::register_node(node_name, ns); + + // Send a heartbeat + autoware_control_center_msgs::msg::NodeStatusActivity status_activity; + status_activity.status = autoware_control_center_msgs::msg::NodeStatusActivity::PROCESSING; + autoware_control_center_msgs::msg::NodeStatusOperational status_operational; + status_operational.status = autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL; + auto pub_hb = test::send_first_heartbeat(node, deadline_ms_, status_activity, status_operational); // Wait for the heartbeat deadline to be missed std::this_thread::sleep_for( std::chrono::duration(deadline_ms_ + node_period_ms_ * 1.5)); + // Expect the node to be marked as dead autoware_control_center_msgs::msg::NodeReport report; - ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report)); + ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report)); ASSERT_FALSE(report.is_alive); ASSERT_EQ( report.status_activity.status, autoware_control_center_msgs::msg::NodeStatusActivity::UNKNOWN); @@ -108,14 +116,13 @@ TEST_F(ControlCenterHeartbeatTest, HeartbeatMissedDeadline) autoware_control_center_msgs::msg::NodeStatusOperational::UNKNOWN); // Send a heartbeat and verify node status is updated - autoware_control_center_msgs::msg::NodeStatusActivity status_activity; status_activity.status = autoware_control_center_msgs::msg::NodeStatusActivity::PROCESSING; - autoware_control_center_msgs::msg::NodeStatusOperational status_operational; status_operational.status = autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL; - test::send_heartbeat(node_name, ns, deadline_ms_, status_activity, status_operational); + test::send_heartbeat(node, pub_hb, status_activity, status_operational); + // Wait shortly and expect the node to be alive std::this_thread::sleep_for(std::chrono::duration(node_period_ms_ * 2.0)); - ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report)); + ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report)); ASSERT_TRUE(report.is_alive); ASSERT_EQ(report.status_activity.status, status_activity.status); ASSERT_EQ(report.status_operational.status, status_operational.status); diff --git a/common/autoware_control_center/test/test_cc_registering.cpp b/common/autoware_control_center/test/test_cc_registering.cpp index 1389b04e..f40ee667 100644 --- a/common/autoware_control_center/test/test_cc_registering.cpp +++ b/common/autoware_control_center/test/test_cc_registering.cpp @@ -62,7 +62,7 @@ TEST_F(ControlCenterRegisteringTest, NodeRegistration) { const std::string node_name = "node_test"; const std::string ns; - const auto [uuid, node_fully_qualified_name] = test::register_node(node_name, ns); + const auto [uuid, node] = test::register_node(node_name, ns); // Make sure node is alive ASSERT_EQ( @@ -71,7 +71,7 @@ TEST_F(ControlCenterRegisteringTest, NodeRegistration) // Make sure node is registered by subscribing to the node reports autoware_control_center_msgs::msg::NodeReport report; - ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report)); + ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report)); ASSERT_EQ(report.uuid, uuid); ASSERT_EQ(report.count_registered, 1); ASSERT_TRUE(report.is_alive); @@ -87,16 +87,16 @@ TEST_F(ControlCenterRegisteringTest, NodeDuplicateRegistration) { const std::string node_name = "node_test_duplicate"; const std::string ns; - const auto [uuid1, node_fully_qualified_name1] = test::register_node(node_name, ns); + const auto [uuid1, node1] = test::register_node(node_name, ns); - const auto [uuid2, node_fully_qualified_name2] = test::register_node(node_name, ns); + const auto [uuid2, node2] = test::register_node(node_name, ns); // Ensure the UUID has changed ASSERT_NE(uuid1, uuid2); // Ensure the registration count incremented by subscribing to node reports autoware_control_center_msgs::msg::NodeReport report; - ASSERT_TRUE(test::wait_for_node_report(node_name, ns, report)); + ASSERT_TRUE(test::wait_for_node_report(node1->get_fully_qualified_name(), report)); ASSERT_EQ(report.count_registered, 2); ASSERT_TRUE(report.is_alive); ASSERT_EQ( @@ -111,11 +111,11 @@ TEST_F(ControlCenterRegisteringTest, NodeDeregistration) { const std::string node_name = "node_test_deregister"; const std::string ns; - const auto [uuid, node_fully_qualified_name] = test::register_node(node_name, ns); + const auto [uuid, node] = test::register_node(node_name, ns); ASSERT_TRUE(test::deregister_node(uuid)); // Ensure node is no longer registered by checking the node reports - ASSERT_FALSE(test::is_node_registered(node_name, ns)); + ASSERT_FALSE(test::is_node_registered(node->get_fully_qualified_name())); } TEST_F(ControlCenterRegisteringTest, DeregisterNonExistentNode) diff --git a/common/autoware_control_center/test/test_utility.hpp b/common/autoware_control_center/test/test_utility.hpp index 496a8405..94a64e2f 100644 --- a/common/autoware_control_center/test/test_utility.hpp +++ b/common/autoware_control_center/test/test_utility.hpp @@ -29,7 +29,7 @@ namespace autoware::control_center::test { -inline std::tuple register_node( +inline std::tuple register_node( const std::string & node_name, const std::string & ns) { auto test_node = std::make_shared(node_name, ns); @@ -61,7 +61,7 @@ inline std::tuple register_node( } RCLCPP_INFO(rclcpp::get_logger("registering"), "Received the response"); - return {result->uuid_node, request->node_name_with_namespace}; + return {result->uuid_node, test_node}; } inline bool deregister_node(const unique_identifier_msgs::msg::UUID & uuid) @@ -92,22 +92,20 @@ inline bool deregister_node(const unique_identifier_msgs::msg::UUID & uuid) } inline bool wait_for_node_report( - const std::string & node_name, const std::string & ns, - autoware_control_center_msgs::msg::NodeReport & report) + const std::string & node_full_name, autoware_control_center_msgs::msg::NodeReport & report) { - auto test_node = std::make_shared(node_name, ns); std::promise report_received; auto future_report = report_received.get_future(); - const std::string fully_qualified_name = test_node->get_fully_qualified_name(); + auto node_temp = std::make_shared("node_report_listener", ""); auto node_reports_sub = - test_node->create_subscription( + node_temp->create_subscription( topic_node_reports, rclcpp::QoS(rclcpp::KeepLast(0)), [&report_received, &report, - &fully_qualified_name](const autoware_control_center_msgs::msg::NodeReports::SharedPtr msg) { + &node_full_name](const autoware_control_center_msgs::msg::NodeReports::SharedPtr msg) { for (const auto & node_report : msg->reports) { - if (node_report.name == fully_qualified_name) { + if (node_report.name == node_full_name) { report = node_report; report_received.set_value(true); return; @@ -117,42 +115,57 @@ inline bool wait_for_node_report( }); if ( - rclcpp::spin_until_future_complete(test_node, future_report, std::chrono::seconds(5)) != + rclcpp::spin_until_future_complete(node_temp, future_report, std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) { return false; } return future_report.get(); } -inline bool is_node_registered(const std::string & node_name, const std::string & ns) +inline bool is_node_registered(const std::string & node_full_name) { autoware_control_center_msgs::msg::NodeReport report; - return wait_for_node_report(node_name, ns, report); + return wait_for_node_report(node_full_name, report); } -inline void send_heartbeat( - const std::string & node_name, const std::string & ns, const double deadline_ms, +inline rclcpp::Publisher::SharedPtr +send_first_heartbeat( + const rclcpp::Node::SharedPtr & node, const double deadline_ms, const autoware_control_center_msgs::msg::NodeStatusActivity & status_activity, const autoware_control_center_msgs::msg::NodeStatusOperational & status_operational) { - auto test_node = std::make_shared(node_name, ns); - - const std::string fully_qualified_name = test_node->get_fully_qualified_name(); + const std::string fully_qualified_name = node->get_fully_qualified_name(); rclcpp::QoS qos_profile(1); qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) .liveliness_lease_duration(std::chrono::duration(deadline_ms)) .deadline(std::chrono::duration(deadline_ms)); - auto pub = test_node->create_publisher( + auto pub = node->create_publisher( fully_qualified_name + test::topic_heartbeat_suffix, qos_profile); autoware_control_center_msgs::msg::Heartbeat heartbeat; - heartbeat.stamp = test_node->now(); + heartbeat.stamp = node->now(); + heartbeat.status_activity = status_activity; + heartbeat.status_operational = status_operational; + + pub->publish(heartbeat); + rclcpp::spin_some(node); + return pub; +} + +inline void send_heartbeat( + const rclcpp::Node::SharedPtr & node, + const rclcpp::Publisher::SharedPtr & pub, + const autoware_control_center_msgs::msg::NodeStatusActivity & status_activity, + const autoware_control_center_msgs::msg::NodeStatusOperational & status_operational) +{ + autoware_control_center_msgs::msg::Heartbeat heartbeat; + heartbeat.stamp = node->now(); heartbeat.status_activity = status_activity; heartbeat.status_operational = status_operational; pub->publish(heartbeat); - rclcpp::spin_some(test_node); + rclcpp::spin_some(node); } } // namespace autoware::control_center::test