register and healthy heartbeat tests

common/autoware_node/config/node.param.yaml

@@ -1,3 +1,5 @@
 /**:
   ros__parameters:
-    period_timer_register_ms: 100
+    period_timer_register_ms: 100.0
+    period_heartbeat_ms: 200.0
+    deadline_ms: 220.0

common/autoware_node/include/autoware/node/node.hpp

@@ -112,23 +112,26 @@ class Node : public rclcpp_lifecycle::LifecycleNode
 
   using FutureRegister = rclcpp::Client<autoware_control_center_msgs::srv::Register>::SharedFuture;
 
-  //  rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_;
 
   rclcpp::Client<autoware_control_center_msgs::srv::Register>::SharedPtr cli_register_;
 
-  //  rclcpp::Publisher<autoware_control_center_msgs::msg::Heartbeat>::SharedPtr pub_heartbeat_;
+  rclcpp::Publisher<autoware_control_center_msgs::msg::Heartbeat>::SharedPtr pub_heartbeat_;
 
-  rclcpp::TimerBase::SharedPtr timer_registration_;
-  //  rclcpp::TimerBase::SharedPtr timer_heartbeat_;
+  uint16_t sequence_number_;
 
   bool is_registered_;
+  double period_timer_register_ms_;
+  double period_heartbeat_ms_;
+  double deadline_ms_;
+
+  rclcpp::TimerBase::SharedPtr timer_registration_;
+  rclcpp::TimerBase::SharedPtr timer_heartbeat_;
+
   unique_identifier_msgs::msg::UUID uuid_node_;
-  //  uint16_t sequence_number_;
-  //  unique_identifier_msgs::msg::UUID self_uuid;
-  //  unique_identifier_msgs::msg::UUID acc_uuid;
 
   void on_tick_registration();
-  //  void on_tick_heartbeat();
+  void on_tick_heartbeat();
   void on_register(FutureRegister future);
 };
 

common/autoware_node/src/node.cpp

@@ -24,37 +24,53 @@
 
 namespace autoware::node
 {
-
 Node::Node(
   const std::string & node_name, const std::string & ns, const rclcpp::NodeOptions & options)
-: LifecycleNode(node_name, ns, options), is_registered_{false}
+  : LifecycleNode(node_name, ns, options),
+    sequence_number_{0},
+    is_registered_{false},
+    period_timer_register_ms_{declare_parameter<double>("period_timer_register_ms")},
+    period_heartbeat_ms_{declare_parameter<double>("period_heartbeat_ms")},
+    deadline_ms_{declare_parameter<double>("deadline_ms")}
 {
   RCLCPP_DEBUG(
     get_logger(), "Node %s constructor", get_node_base_interface()->get_fully_qualified_name());
-  std::chrono::milliseconds period_timer_register(
-    declare_parameter<int>("period_timer_register_ms"));
 
-  //  std::chrono::milliseconds heartbeat_period(declare_parameter<int>("heartbeat_period"));
+  rclcpp::QoS qos_profile(1);
+  qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC)
+             .liveliness_lease_duration(std::chrono::duration<double, std::milli>(deadline_ms_))
+             .deadline(std::chrono::duration<double, std::milli>(deadline_ms_));
 
-  //  // The granted lease is essentially infinite here, i.e., only reader/watchdog will notify
-  //  // violations. XXX causes segfault for cyclone dds, hence pass explicit lease life >
-  //  heartbeat. rclcpp::QoS qos_profile(1);
-  //  qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC)
-  //    .liveliness_lease_duration(heartbeat_period + lease_delta)
-  //    .deadline(heartbeat_period + lease_delta);
-  //
-  //  pub_heartbeat_ = this->create_publisher<autoware_control_center_msgs::msg::Heartbeat>(
-  //    "~/heartbeat", qos_profile);
-  //  timer_heartbeat_ =
-  //    this->create_wall_timer(heartbeat_period, std::bind(&Node::on_tick_heartbeat, this));
+  pub_heartbeat_ = this->create_publisher<autoware_control_center_msgs::msg::Heartbeat>(
+    "~/heartbeat", qos_profile);
+  timer_heartbeat_ =
+    this->create_wall_timer(
+      std::chrono::duration<double, std::milli>(period_heartbeat_ms_),
+      std::bind(&Node::on_tick_heartbeat, this));
 
-  //  callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
   cli_register_ = create_client<autoware_control_center_msgs::srv::Register>(
     "/autoware/control_center/srv/register", rmw_qos_profile_default);
 
   timer_registration_ =
-    this->create_wall_timer(period_timer_register, std::bind(&Node::on_tick_registration, this));
+    this->create_wall_timer(
+      std::chrono::duration<double, std::milli>(period_timer_register_ms_),
+      std::bind(&Node::on_tick_registration, this));
+}
+
+
+void Node::on_tick_heartbeat()
+{
+  auto heartbeat = std::make_shared<autoware_control_center_msgs::msg::Heartbeat>();
+  heartbeat->stamp = rclcpp::Clock().now();
+  heartbeat->status_activity.status =
+    autoware_control_center_msgs::msg::NodeStatusActivity::PROCESSING;
+  heartbeat->status_operational.status =
+    autoware_control_center_msgs::msg::NodeStatusOperational::NORMAL;
+  heartbeat->sequence_number = sequence_number_;
+  pub_heartbeat_->publish(*heartbeat);
+  sequence_number_++;
 }
 
 void Node::on_tick_registration()
@@ -94,5 +110,4 @@ void Node::on_register(FutureRegister future)
   is_registered_ = true;
   RCLCPP_DEBUG(get_logger(), "Registration succeeded.");
 }
-
-}  // namespace autoware::node
+} // namespace autoware::node

common/autoware_node/test/test_an_heartbeat.cpp

@@ -0,0 +1,102 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <autoware/control_center/control_center_node.hpp>
+#include <autoware/node/node.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <autoware_utils/ros/uuid_helper.hpp>
+#include <lifecycle_msgs/msg/state.hpp>
+#include <gtest/gtest.h>
+
+#include "test_utility.hpp"
+
+namespace autoware::node
+{
+class AutowareNodeHeartbeat : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    node_options_an_.append_parameter_override("period_timer_register_ms", 100.0);
+    node_options_an_.append_parameter_override("period_heartbeat_ms", period_heartbeat_ms_);
+    node_options_an_.append_parameter_override("deadline_ms", deadline_ms_);
+
+    node_options_acc_.append_parameter_override("deadline_ms", deadline_ms_);
+    node_options_acc_.append_parameter_override(
+      "report_publish_rate", 1000.0 / period_report_publish_ms_);
+  }
+
+  void TearDown() override { rclcpp::shutdown(); }
+
+  static void validate_heartbeat(
+    const std::string & node_full_name,
+    const autoware_control_center_msgs::msg::Heartbeat::ConstSharedPtr & hb_healthy)
+  {
+    autoware_control_center_msgs::msg::NodeReport report;
+    ASSERT_TRUE(test::wait_for_node_report(node_full_name, report));
+    ASSERT_EQ(report.status_activity.status, hb_healthy->status_activity.status);
+    ASSERT_EQ(report.status_operational.status, hb_healthy->status_operational.status);
+  }
+
+  rclcpp::NodeOptions node_options_an_;
+  rclcpp::NodeOptions node_options_acc_;
+
+  double deadline_ms_{220.0};
+  double period_report_publish_ms_{10.0};
+  double period_heartbeat_ms_{200.0};
+};
+
+TEST_F(AutowareNodeHeartbeat, HeartbeatHealthy)
+{
+  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor;
+  executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  autoware::node::Node::SharedPtr autoware_node =
+    std::make_shared<autoware::node::Node>("test_node", "test_ns", node_options_an_);
+  autoware::node::Node::SharedPtr control_center =
+    std::make_shared<autoware::control_center::ControlCenter>(node_options_acc_);
+
+  executor->add_node(control_center->get_node_base_interface());
+  std::thread thread_spin = std::thread([&executor]() { executor->spin(); });
+  // wait until executor is actually spinning
+  while (!executor->is_spinning()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+
+  // let ACC initialize
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  executor->add_node(autoware_node->get_node_base_interface());
+
+  // wait until registered and published at least one heartbeat
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(period_heartbeat_ms_));
+
+  for (int i = 0; i < 3; ++i) {
+    {
+      SCOPED_TRACE("validate_heartbeat iteration " + std::to_string(i));
+      validate_heartbeat(
+        autoware_node->get_node_base_interface()->get_fully_qualified_name(),
+        test::generate_hb_healthy());
+      std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(period_heartbeat_ms_));
+    }
+  }
+
+  executor->cancel(); // make sure cancel is called after spin
+  if (thread_spin.joinable()) {
+    thread_spin.join();
+  }
+}
+} // namespace autoware::node

common/autoware_node/test/test_an_init_shutdown.cpp

@@ -26,16 +26,19 @@ class AutowareNodeInitShutdown : public ::testing::Test
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-    node_options_an_.append_parameter_override("period_timer_register_ms", 100);
+    double deadline_ms = 220.0;
+    node_options_an_.append_parameter_override("period_timer_register_ms", 100.0);
+    node_options_an_.append_parameter_override("period_heartbeat_ms", 100.0);
+    node_options_an_.append_parameter_override("deadline_ms", deadline_ms);
 
-    node_options_acc_.append_parameter_override("deadline_ms", 220.0);
+    node_options_acc_.append_parameter_override("deadline_ms", deadline_ms);
     node_options_acc_.append_parameter_override("report_publish_rate", 100.0);
   }
 
   void TearDown() override { rclcpp::shutdown(); }
 
-  rclcpp::NodeOptions node_options_acc_;
   rclcpp::NodeOptions node_options_an_;
+  rclcpp::NodeOptions node_options_acc_;
 };
 
 TEST_F(AutowareNodeInitShutdown, NodeInitShutdown)

common/autoware_node/test/test_an_registering.cpp

@@ -15,27 +15,33 @@
 #include <autoware/control_center/control_center_node.hpp>
 #include <autoware/node/node.hpp>
 #include <rclcpp/rclcpp.hpp>
-
+#include <autoware_utils/ros/uuid_helper.hpp>
 #include <lifecycle_msgs/msg/state.hpp>
-
 #include <gtest/gtest.h>
 
+#include "test_utility.hpp"
+
+namespace autoware::node
+{
 class AutowareNodeRegistering : public ::testing::Test
 {
 public:
   void SetUp() override
   {
     rclcpp::init(0, nullptr);
-    node_options_an_.append_parameter_override("period_timer_register_ms", 100);
+    double deadline_ms = 220.0;
+    node_options_an_.append_parameter_override("period_timer_register_ms", 100.0);
+    node_options_an_.append_parameter_override("period_heartbeat_ms", 100.0);
+    node_options_an_.append_parameter_override("deadline_ms", deadline_ms);
 
-    node_options_acc_.append_parameter_override("deadline_ms", 220.0);
+    node_options_acc_.append_parameter_override("deadline_ms", deadline_ms);
     node_options_acc_.append_parameter_override("report_publish_rate", 100.0);
   }
 
   void TearDown() override { rclcpp::shutdown(); }
 
-  rclcpp::NodeOptions node_options_acc_;
   rclcpp::NodeOptions node_options_an_;
+  rclcpp::NodeOptions node_options_acc_;
 };
 
 TEST_F(AutowareNodeRegistering, RegisterSuccessFirstACCThenAN)
@@ -50,27 +56,76 @@ TEST_F(AutowareNodeRegistering, RegisterSuccessFirstACCThenAN)
 
   executor->add_node(control_center->get_node_base_interface());
   std::thread thread_spin = std::thread([&executor]() { executor->spin(); });
+  // wait until executor is actually spinning
+  while (!executor->is_spinning()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
 
+  // let ACC initialize
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
   executor->add_node(autoware_node->get_node_base_interface());
 
-  ASSERT_EQ(
-    autoware_node->get_current_state().id(),
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  // wait until registered
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
-  auto state = autoware_node->shutdown();
+  // 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(autoware_node->get_node_base_interface()->get_fully_qualified_name(),
+      report));
 
-  ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+  // print uuid
+  std::cout << "uuid: " << autoware_utils::to_hex_string(report.uuid) << std::endl;
+  ASSERT_EQ(report.count_registered, 1);
+  ASSERT_TRUE(report.is_alive);
 
-  // wait until executor is spinning
+  executor->cancel(); // make sure cancel is called after spin
+  if (thread_spin.joinable()) {
+    thread_spin.join();
+  }
+}
+
+
+TEST_F(AutowareNodeRegistering, RegisterSuccessFirstANThenACC)
+{
+  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor;
+  executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  autoware::node::Node::SharedPtr autoware_node =
+    std::make_shared<autoware::node::Node>("test_node", "test_ns", node_options_an_);
+  autoware::node::Node::SharedPtr control_center =
+    std::make_shared<autoware::control_center::ControlCenter>(node_options_acc_);
+
+  executor->add_node(autoware_node->get_node_base_interface());
+  std::thread thread_spin = std::thread([&executor]() { executor->spin(); });
+  // wait until executor is actually spinning
   while (!executor->is_spinning()) {
     std::this_thread::sleep_for(std::chrono::milliseconds(1));
   }
-  executor->cancel();  // make sure cancel is called after spin
+
+  // let AN initialize
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  executor->add_node(control_center->get_node_base_interface());
+
+
+  // wait until registered
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  // 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(autoware_node->get_node_base_interface()->get_fully_qualified_name(),
+      report));
+
+  // print uuid
+  std::cout << "uuid: " << autoware_utils::to_hex_string(report.uuid) << std::endl;
+  ASSERT_EQ(report.count_registered, 1);
+  ASSERT_TRUE(report.is_alive);
+
+  executor->cancel(); // make sure cancel is called after spin
   if (thread_spin.joinable()) {
     thread_spin.join();
   }
 }
-
-// TEST_F(AutowareNodeTest, Sen)
+} // namespace autoware::node