From 6d8552839fd28677ea1b64f99ae30522a64c9632 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Wed, 27 Nov 2024 15:38:16 +0300 Subject: [PATCH] feat: add autoware_control_center and autoware_node MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- common/autoware_control_center/CMakeLists.txt | 52 ++++ common/autoware_control_center/README.md | 100 +++++++ .../config/control_center.param.yaml | 4 + .../control_center/control_center_node.hpp | 116 ++++++++ .../launch/control_center.launch.xml | 8 + common/autoware_control_center/package.xml | 34 +++ .../src/control_center_main.cpp | 122 +++++++++ .../src/control_center_node.cpp | 254 ++++++++++++++++++ .../test/config/control_center.param.yaml | 4 + .../test/test_cc_heartbeat.cpp | 142 ++++++++++ .../test/test_cc_init_shutdown.cpp | 59 ++++ .../test/test_cc_registering.cpp | 133 +++++++++ .../test/test_constants.hpp | 26 ++ .../test/test_utility.hpp | 173 ++++++++++++ .../CMakeLists.txt | 28 ++ common/autoware_control_center_msgs/README.md | 3 + .../msg/Heartbeat.msg | 17 ++ .../msg/NodeReport.msg | 15 ++ .../msg/NodeReports.msg | 4 + .../msg/NodeStatusActivity.msg | 10 + .../msg/NodeStatusOperational.msg | 9 + .../msg/ResultDeregistration.msg | 7 + .../msg/ResultRegistration.msg | 6 + .../autoware_control_center_msgs/package.xml | 26 ++ .../srv/Deregister.srv | 3 + .../srv/Register.srv | 4 + common/autoware_node/CMakeLists.txt | 30 +++ common/autoware_node/README.md | 72 +++++ common/autoware_node/config/node.param.yaml | 5 + .../include/autoware/node/node.hpp | 88 ++++++ .../autoware/node/visibility_control.hpp | 26 ++ common/autoware_node/package.xml | 24 ++ common/autoware_node/src/node.cpp | 126 +++++++++ .../autoware_node/test/test_an_heartbeat.cpp | 104 +++++++ .../test/test_an_init_shutdown.cpp | 77 ++++++ .../test/test_an_registering.cpp | 129 +++++++++ common/autoware_node/test/test_constants.hpp | 26 ++ common/autoware_node/test/test_utility.hpp | 173 ++++++++++++ common/autoware_test_node/CMakeLists.txt | 15 ++ common/autoware_test_node/README.md | 58 ++++ .../launch/autoware_node_with_cc.launch.xml | 21 ++ common/autoware_test_node/package.xml | 27 ++ .../src/include/test_node.hpp | 32 +++ common/autoware_test_node/src/test_node.cpp | 31 +++ .../test/test_test_node.cpp | 15 ++ .../test/test_test_node.hpp | 18 ++ 46 files changed, 2456 insertions(+) create mode 100644 common/autoware_control_center/CMakeLists.txt create mode 100644 common/autoware_control_center/README.md create mode 100644 common/autoware_control_center/config/control_center.param.yaml create mode 100644 common/autoware_control_center/include/autoware/control_center/control_center_node.hpp create mode 100755 common/autoware_control_center/launch/control_center.launch.xml create mode 100644 common/autoware_control_center/package.xml create mode 100644 common/autoware_control_center/src/control_center_main.cpp create mode 100644 common/autoware_control_center/src/control_center_node.cpp create mode 100644 common/autoware_control_center/test/config/control_center.param.yaml create mode 100644 common/autoware_control_center/test/test_cc_heartbeat.cpp create mode 100644 common/autoware_control_center/test/test_cc_init_shutdown.cpp create mode 100644 common/autoware_control_center/test/test_cc_registering.cpp create mode 100644 common/autoware_control_center/test/test_constants.hpp create mode 100644 common/autoware_control_center/test/test_utility.hpp create mode 100644 common/autoware_control_center_msgs/CMakeLists.txt create mode 100644 common/autoware_control_center_msgs/README.md create mode 100644 common/autoware_control_center_msgs/msg/Heartbeat.msg create mode 100644 common/autoware_control_center_msgs/msg/NodeReport.msg create mode 100644 common/autoware_control_center_msgs/msg/NodeReports.msg create mode 100644 common/autoware_control_center_msgs/msg/NodeStatusActivity.msg create mode 100644 common/autoware_control_center_msgs/msg/NodeStatusOperational.msg create mode 100644 common/autoware_control_center_msgs/msg/ResultDeregistration.msg create mode 100644 common/autoware_control_center_msgs/msg/ResultRegistration.msg create mode 100644 common/autoware_control_center_msgs/package.xml create mode 100644 common/autoware_control_center_msgs/srv/Deregister.srv create mode 100644 common/autoware_control_center_msgs/srv/Register.srv create mode 100644 common/autoware_node/CMakeLists.txt create mode 100644 common/autoware_node/README.md create mode 100644 common/autoware_node/config/node.param.yaml create mode 100644 common/autoware_node/include/autoware/node/node.hpp create mode 100644 common/autoware_node/include/autoware/node/visibility_control.hpp create mode 100644 common/autoware_node/package.xml create mode 100644 common/autoware_node/src/node.cpp create mode 100644 common/autoware_node/test/test_an_heartbeat.cpp create mode 100644 common/autoware_node/test/test_an_init_shutdown.cpp create mode 100644 common/autoware_node/test/test_an_registering.cpp create mode 100644 common/autoware_node/test/test_constants.hpp create mode 100644 common/autoware_node/test/test_utility.hpp create mode 100644 common/autoware_test_node/CMakeLists.txt create mode 100644 common/autoware_test_node/README.md create mode 100644 common/autoware_test_node/launch/autoware_node_with_cc.launch.xml create mode 100644 common/autoware_test_node/package.xml create mode 100644 common/autoware_test_node/src/include/test_node.hpp create mode 100644 common/autoware_test_node/src/test_node.cpp create mode 100644 common/autoware_test_node/test/test_test_node.cpp create mode 100644 common/autoware_test_node/test/test_test_node.hpp diff --git a/common/autoware_control_center/CMakeLists.txt b/common/autoware_control_center/CMakeLists.txt new file mode 100644 index 00000000..0fb14efc --- /dev/null +++ b/common/autoware_control_center/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_control_center) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/control_center_node.cpp + include/autoware/control_center/control_center_node.hpp +) + +# In order to instantiate only a single instance, we have to use custom main function +#rclcpp_components_register_node(${PROJECT_NAME} +# PLUGIN "autoware::control_center::ControlCenter" +# EXECUTABLE ${PROJECT_NAME}_node +#) + +ament_auto_add_executable(${PROJECT_NAME}_node + src/control_center_main.cpp +) + +if(BUILD_TESTING) + add_smoke_test(${PROJECT_NAME} ${PROJECT_NAME}_node + TEST_PARAM_FILENAMES "config/control_center.param.yaml" + ) + + install(DIRECTORY + test/config/ + DESTINATION share/${PROJECT_NAME}/test/config/ + ) + + file(GLOB_RECURSE TEST_FILES test/*.cpp) + + foreach(TEST_FILE ${TEST_FILES}) + # Get the test name without directory and extension + get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) + + # Add each test separately + ament_add_ros_isolated_gtest(${TEST_NAME} ${TEST_FILE} TIMEOUT 10) + target_include_directories(${TEST_NAME} PRIVATE src/include) + target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) + ament_target_dependencies(${TEST_NAME} + rclcpp + rclcpp_lifecycle + autoware_utils + autoware_control_center_msgs) + endforeach() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config) diff --git a/common/autoware_control_center/README.md b/common/autoware_control_center/README.md new file mode 100644 index 00000000..aa8bf553 --- /dev/null +++ b/common/autoware_control_center/README.md @@ -0,0 +1,100 @@ +# Autoware Control Center + +## Abbreviations + +- **ACC:** Autoware Control Center +- **AN:** Autoware Node +- **HB:** Heartbeat + +## Overview + +The Autoware Control Center (ACC) is a core package within the Autoware system, designed to manage and monitor Autoware +nodes (ANs). + +It provides services for the registration, de-registration, and error handling of ANs, as well as publishing reports on +their status. + +ACC maintains an internal registry to keep track of registered ANs. + +## Interfaces + +### Services + +#### Register + +Registers an Autoware node to the ACC. + +- **Topic:** `/autoware/control_center/srv/register` +- **Type:** `autoware_control_center_msgs::srv::Register` + +#### Deregister + +De-registers an Autoware node from the ACC. + +- **Topic:** `/autoware/control_center/srv/deregister` +- **Type:** `autoware_control_center_msgs::srv::Deregister` + +### Subscribers + +#### Heartbeat (**Source:** Autoware Node) + +Subscribes to the heartbeat topic of an Autoware node after its registration. +ACC controls the liveness of Autoware nodes by monitoring this topic. + +This topic also contains the information about the node's status. + +- **Topic:** `/autoware_node_name/heartbeat` +- **Type:** `autoware_control_center_msgs::msg::Heartbeat` +- **Source:** An Autoware Node + +### Publishers + +#### NodeReports + +Publishes reports on the current status of registered Autoware nodes. + +- **Topic:** `/autoware/control_center/node_reports` +- **Type:** `autoware_control_center_msgs::msg::NodeReports` + +## Parameters + +| Name | Type | Default Value | Description | +| --------------------- | -------- | ------------- | ------------------------------------------------------------------------ | +| `deadline_ms` | `double` | `220.0` | Maximum duration to wait for a heartbeat before considering a node dead. | +| `report_publish_rate` | `double` | `1.0` | Frequency (in Hz) at which NodeReports are published. | + +## Singleton Constraint + +To ensure that only one instance of the ACC is running at any given time, two checks are performed in the main function: + +### 1. Lockfile Check + +This lockfile mechanism is fast and reliable for single-machine scenarios. +It prevents multiple instances of ACC from running concurrently on the same machine. + +**Path:** `/tmp/autoware_control_center_node.lock` + +### 2. Network-Wide Node Name Check + +This involves gathering all node names and comparing them with the ACC node name (`/autoware/control_center`). +While this method is slower and less reliable than the lockfile check, +it is necessary for scenarios where the ACC is run across a network of machines. +This ensures that no other instance of ACC is running on any other machine within the network. + +## Usage + +`ros2 launch autoware_control_center control_center.launch.xml` + +## Workflow + +When an Autoware Node starts, it registers itself with the ACC. + +The ACC then subscribes to the heartbeat topic of the Autoware node to monitor its liveliness. + +If the ACC does not receive a heartbeat from the Autoware node within the specified deadline, +it considers the node to be dead. + +## Credits + +- Heartbeat functionality is based on ROS 2 [software_watchdogs](https://github.com/ros-safety/software_watchdogs) + package. diff --git a/common/autoware_control_center/config/control_center.param.yaml b/common/autoware_control_center/config/control_center.param.yaml new file mode 100644 index 00000000..fe6f644c --- /dev/null +++ b/common/autoware_control_center/config/control_center.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + deadline_ms: 220.0 + report_publish_rate: 1.0 diff --git a/common/autoware_control_center/include/autoware/control_center/control_center_node.hpp b/common/autoware_control_center/include/autoware/control_center/control_center_node.hpp new file mode 100644 index 00000000..e2a1647b --- /dev/null +++ b/common/autoware_control_center/include/autoware/control_center/control_center_node.hpp @@ -0,0 +1,116 @@ +// 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. + +#ifndef AUTOWARE__CONTROL_CENTER__CONTROL_CENTER_NODE_HPP_ +#define AUTOWARE__CONTROL_CENTER__CONTROL_CENTER_NODE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::control_center +{ + +class ControlCenter : public rclcpp_lifecycle::LifecycleNode +{ +public: + using SharedPtr = std::shared_ptr; + using NodeReport = autoware_control_center_msgs::msg::NodeReport; + explicit ControlCenter(const rclcpp::NodeOptions & options); + + struct NodeInfo + { + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = std::shared_ptr; + + NodeReport report; + rclcpp::Subscription::SharedPtr heartbeat_sub; + }; + +private: + using Heartbeat = autoware_control_center_msgs::msg::Heartbeat; + using NodeReports = autoware_control_center_msgs::msg::NodeReports; + using NodeStatusActivity = autoware_control_center_msgs::msg::NodeStatusActivity; + using NodeStatusOperational = autoware_control_center_msgs::msg::NodeStatusOperational; + using ResultDeregistration = autoware_control_center_msgs::msg::ResultDeregistration; + using ResultRegistration = autoware_control_center_msgs::msg::ResultRegistration; + using Deregister = autoware_control_center_msgs::srv::Deregister; + using Register = autoware_control_center_msgs::srv::Register; + + std::tuple> register_node( + const std::string & node_name); + ControlCenter::ResultDeregistration deregister_node( + const unique_identifier_msgs::msg::UUID & uuid); + + bool is_registered(const unique_identifier_msgs::msg::UUID & uuid) const; + bool is_registered(const std::string & name) const; + + /// @brief Retrieves the UUID of a registered node. + /// @note Ensure the node is registered using `is_registered(node_name)` before calling this + /// function. + unique_identifier_msgs::msg::UUID get_uuid(const std::string & node_name) const; + + /// @brief Retrieves the NodeInfo of a registered node. + /// @note Ensure the node is registered using `is_registered(uuid)` before calling this function. + ControlCenter::NodeInfo::ConstSharedPtr get_node_info( + const unique_identifier_msgs::msg::UUID & uuid) const; + + void on_heartbeat( + const autoware_control_center_msgs::msg::Heartbeat::SharedPtr heartbeat, + const unique_identifier_msgs::msg::UUID & uuid); + + void on_deadline_missed( + rclcpp::QOSDeadlineRequestedInfo & event, const unique_identifier_msgs::msg::UUID & uuid); + + void on_liveliness_changed( + rclcpp::QOSLivelinessChangedInfo & event, const unique_identifier_msgs::msg::UUID & uuid); + + void publish_node_reports(); + + void on_register_node( + const std::shared_ptr request, std::shared_ptr response); + + void on_deregister_node( + const std::shared_ptr request, + std::shared_ptr response); + + rclcpp::Service::SharedPtr srv_register_; + rclcpp::Service::SharedPtr srv_deregister_; + rclcpp::Publisher::SharedPtr pub_reports_; + rclcpp::TimerBase::SharedPtr timer_publish_reports_; + + std::map node_name_to_uuid_; + std::map, NodeInfo::SharedPtr> uuid_to_info_; + + double deadline_ms_; + double report_publish_rate_; +}; + +} // namespace autoware::control_center + +#endif // AUTOWARE__CONTROL_CENTER__CONTROL_CENTER_NODE_HPP_ diff --git a/common/autoware_control_center/launch/control_center.launch.xml b/common/autoware_control_center/launch/control_center.launch.xml new file mode 100755 index 00000000..df115504 --- /dev/null +++ b/common/autoware_control_center/launch/control_center.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/common/autoware_control_center/package.xml b/common/autoware_control_center/package.xml new file mode 100644 index 00000000..0ad1eb0f --- /dev/null +++ b/common/autoware_control_center/package.xml @@ -0,0 +1,34 @@ + + + + autoware_control_center + 0.0.0 + + Autoware Control Center (ACC) is an Autoware.Core package designed to manage + and monitor Autoware nodes within a system. + + M. Fatih Cırıt + Apache-2.0 + + ament_cmake_auto + autoware_cmake + + autoware_control_center_msgs + autoware_utils + lifecycle_msgs + rclcpp + rclcpp_components + rclcpp_lifecycle + + ament_cmake_gtest + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_testing + autoware_utils + ros_testing + + + ament_cmake + + diff --git a/common/autoware_control_center/src/control_center_main.cpp b/common/autoware_control_center/src/control_center_main.cpp new file mode 100644 index 00000000..eaf63b99 --- /dev/null +++ b/common/autoware_control_center/src/control_center_main.cpp @@ -0,0 +1,122 @@ +// 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 + +#include +#include + +#include +#include +#include + +const char * lock_file_path = "/tmp/autoware_control_center_node.lock"; + +int open_lock_file() +{ + // Open or create the lock file with read/write permissions + int fd = open(lock_file_path, O_CREAT | O_RDWR, 0666); + + // Check if the file descriptor is valid + if (fd == -1) { + RCLCPP_FATAL(rclcpp::get_logger(""), "Failed to open lock file"); + } + return fd; +} + +bool lock_file(int fd) +{ + struct flock fl = {F_WRLCK, SEEK_SET, 0, 0, 0}; + // F_WRLCK: Write lock + // SEEK_SET: Base the lock offset from the beginning of the file + // 0, 0: Lock the entire file (start offset, length) + + // Attempt to set the file lock using fcntl + if (fcntl(fd, F_SETLK, &fl) == -1) { + // Check if the file is already locked by another process + if (errno == EWOULDBLOCK) { + RCLCPP_FATAL(rclcpp::get_logger(""), "Another instance is already running"); + } else { + // Handle other locking errors + RCLCPP_FATAL(rclcpp::get_logger(""), "Failed to lock file"); + } + // Close the file descriptor on failure + close(fd); + return false; + } + return true; +} + +void unlock_file(int fd) +{ + // Ensure the file descriptor is valid before closing + if (fd != -1) { + close(fd); + } +} + +bool node_already_exists(const std::string & node_name) +{ + auto temp_node = rclcpp::Node::make_shared("temp_node"); + auto all_nodes = temp_node->get_node_names(); + return std::find(all_nodes.begin(), all_nodes.end(), node_name) != all_nodes.end(); +} + +int main(int argc, char * argv[]) +{ + bool use_lock_file = true; + for (int i = 1; i < argc; ++i) { + if (std::string(argv[i]) == "--dont-use-lock-file") { + use_lock_file = false; + break; + } + } + rclcpp::init(argc, argv); + + // Lock file to prevent multiple instances on the same machine + int lock_fd = -1; + if (use_lock_file) { + lock_fd = open_lock_file(); + if (lock_fd == -1 || !lock_file(lock_fd)) { + return 1; + } + } + + // Check if node already exists to prevent multiple instances across the network + // It's a bit slow but better than nothing + const std::string node_name_with_namespace = "/autoware/control_center"; + if (node_already_exists(node_name_with_namespace)) { + RCLCPP_FATAL( + rclcpp::get_logger(""), "Node %s already exists", node_name_with_namespace.c_str()); + if (use_lock_file) { + unlock_file(lock_fd); + } + return 2; + } + + // Instantiate the control center node, hopefully the only instance + auto control_center = + std::make_shared(rclcpp::NodeOptions()); + rclcpp::executors::MultiThreadedExecutor exec; + exec.add_node(control_center->get_node_base_interface()); + exec.spin(); + + if (use_lock_file) { + unlock_file(lock_fd); + } + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_control_center/src/control_center_node.cpp b/common/autoware_control_center/src/control_center_node.cpp new file mode 100644 index 00000000..af352cd0 --- /dev/null +++ b/common/autoware_control_center/src/control_center_node.cpp @@ -0,0 +1,254 @@ +// 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 + +#include + +#include + +namespace autoware::control_center +{ + +ControlCenter::ControlCenter(const rclcpp::NodeOptions & options) +: LifecycleNode("control_center", "autoware", options), + deadline_ms_{declare_parameter("deadline_ms")}, + report_publish_rate_{declare_parameter("report_publish_rate")} +{ + using std::placeholders::_1; + using std::placeholders::_2; + + const rclcpp::QoS qos_profile = + rclcpp::QoS(rclcpp::KeepLast(999)).reliable().durability_volatile(); + const rmw_qos_profile_t rmw_qos_profile = qos_profile.get_rmw_qos_profile(); + + srv_register_ = create_service( + "~/srv/register", std::bind(&ControlCenter::on_register_node, this, _1, _2), rmw_qos_profile); + srv_deregister_ = create_service( + "~/srv/deregister", std::bind(&ControlCenter::on_deregister_node, this, _1, _2), + rmw_qos_profile); + + timer_publish_reports_ = create_wall_timer( + std::chrono::duration(1.0 / report_publish_rate_), + std::bind(&ControlCenter::publish_node_reports, this)); + + pub_reports_ = create_publisher("~/node_reports", 10); +} + +std::tuple> +ControlCenter::register_node(const std::string & node_name) +{ + RCLCPP_DEBUG(this->get_logger(), "Registering node %s", node_name.c_str()); + + auto node_info = std::make_shared(); + + auto & report = node_info->report; + + if (is_registered(node_name)) { + const auto & uuid_old = get_uuid(node_name); + if (!is_registered(uuid_old)) { + // This should not be possible + throw std::logic_error("Node name is registered but UUID is not"); + } + const auto & node_info_old = get_node_info(uuid_old); + report.count_registered = node_info_old->report.count_registered; + deregister_node(uuid_old); + } + + report.count_registered++; + report.name = node_name; + report.uuid = autoware_utils::generate_uuid(); + report.stamp_registration = rclcpp::Clock().now(); + report.is_alive = true; + report.status_activity.status = NodeStatusActivity::INITIALIZING; + report.status_operational.status = NodeStatusOperational::WARNING; + report.stamp_last_heartbeat = rclcpp::Clock().now(); + node_name_to_uuid_[node_name] = node_info->report.uuid; + uuid_to_info_[node_info->report.uuid.uuid] = node_info; + + const std::chrono::milliseconds deadline_duration{static_cast(deadline_ms_)}; + std::string topic_name = node_name + "/heartbeat"; + // auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10)).deadline(deadline_duration); + auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10)) + .liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + .liveliness_lease_duration(deadline_duration); + + std::function + on_heartbeat_wrapped = + std::bind(&ControlCenter::on_heartbeat, this, std::placeholders::_1, node_info->report.uuid); + + rclcpp::SubscriptionOptions heartbeat_sub_options; + // auto bound_on_deadline = std::bind( + // &ControlCenter::on_deadline_missed, this, std::placeholders::_1, node_info->report.uuid); + // heartbeat_sub_options.event_callbacks.deadline_callback = bound_on_deadline; + auto bound_on_liveliness = std::bind( + &ControlCenter::on_liveliness_changed, this, std::placeholders::_1, node_info->report.uuid); + heartbeat_sub_options.event_callbacks.liveliness_callback = bound_on_liveliness; + + node_info->heartbeat_sub = + this->create_subscription( + topic_name, qos_profile, on_heartbeat_wrapped, heartbeat_sub_options); + + ResultRegistration result_registration; + result_registration.result = ResultRegistration::SUCCESS; + return std::make_tuple(result_registration, node_info->report.uuid); +} + +ControlCenter::ResultDeregistration ControlCenter::deregister_node( + const unique_identifier_msgs::msg::UUID & uuid) +{ + ResultDeregistration result; + if (!is_registered(uuid)) { + RCLCPP_WARN(this->get_logger(), "Attempt to deregister non-existent node with UUID"); + result.result = ResultDeregistration::FAILURE_NOT_REGISTERED; + return result; + } + + auto node_info = uuid_to_info_.at(uuid.uuid); + const auto & node_name = node_info->report.name; + RCLCPP_DEBUG(this->get_logger(), "Deregistering node %s", node_name.c_str()); + + uuid_to_info_.erase(uuid.uuid); + + if (!is_registered(node_name)) { + // This should not be possible + throw std::logic_error("Node name is registered but UUID is not"); + } + node_name_to_uuid_.erase(node_name); + + result.result = ResultDeregistration::SUCCESS; + return result; +} + +bool ControlCenter::is_registered(const unique_identifier_msgs::msg::UUID & uuid) const +{ + return uuid_to_info_.find(uuid.uuid) != uuid_to_info_.end(); +} + +bool ControlCenter::is_registered(const std::string & name) const +{ + return node_name_to_uuid_.find(name) != node_name_to_uuid_.end(); +} + +ControlCenter::NodeInfo::ConstSharedPtr ControlCenter::get_node_info( + const unique_identifier_msgs::msg::UUID & uuid) const +{ + return uuid_to_info_.at(uuid.uuid); +} + +unique_identifier_msgs::msg::UUID ControlCenter::get_uuid(const std::string & node_name) const +{ + return node_name_to_uuid_.at(node_name); +} + +void ControlCenter::on_heartbeat( + const autoware_control_center_msgs::msg::Heartbeat::SharedPtr heartbeat, + const unique_identifier_msgs::msg::UUID & uuid) +{ + if (!is_registered(uuid)) { + RCLCPP_WARN(this->get_logger(), "Received heartbeat for unregistered node"); + return; + } + auto node_info = uuid_to_info_[uuid.uuid]; + 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( + rclcpp::QOSDeadlineRequestedInfo & event, const unique_identifier_msgs::msg::UUID & uuid) +{ + if (!is_registered(uuid)) { + RCLCPP_WARN(this->get_logger(), "Deadline missed for unregistered node"); + return; + } + + auto & node_report = uuid_to_info_.at(uuid.uuid)->report; + node_report.is_alive = false; + node_report.status_activity.status = NodeStatusActivity::UNKNOWN; + node_report.status_operational.status = NodeStatusOperational::UNKNOWN; + RCLCPP_WARN( + this->get_logger(), "Deadline missed for node %s: total_count=%d, total_count_change=%d", + node_report.name.c_str(), event.total_count, event.total_count_change); +} + +void ControlCenter::on_liveliness_changed( + rclcpp::QOSLivelinessChangedInfo & event, const unique_identifier_msgs::msg::UUID & uuid) +{ + if (!is_registered(uuid)) { + RCLCPP_WARN(this->get_logger(), "Liveliness changed for node for unregistered node"); + return; + } + + auto & node_report = uuid_to_info_.at(uuid.uuid)->report; + RCLCPP_WARN( + this->get_logger(), "Liveliness changed for node %s: alive_count=%d, not_alive_count=%d", + node_report.name.c_str(), event.alive_count, event.not_alive_count); + + // Only consider the alive -> not alive transition + if (event.alive_count != 0) return; + + RCLCPP_DEBUG(this->get_logger(), "Node %s is not alive", node_report.name.c_str()); + node_report.is_alive = false; + node_report.status_activity.status = NodeStatusActivity::UNKNOWN; + node_report.status_operational.status = NodeStatusOperational::UNKNOWN; +} + +void ControlCenter::publish_node_reports() +{ + NodeReports node_reports_msg; + node_reports_msg.stamp = rclcpp::Clock().now(); + node_reports_msg.reports.resize(uuid_to_info_.size()); + + std::transform( + uuid_to_info_.begin(), uuid_to_info_.end(), node_reports_msg.reports.begin(), + [](const auto & entry) { return entry.second->report; }); + + pub_reports_->publish(node_reports_msg); +} + +void ControlCenter::on_register_node( + const std::shared_ptr request, std::shared_ptr response) +{ + const auto & node_name = request->node_name_with_namespace; + auto [result_registration, node_uuid] = register_node(node_name); + + response->result_registration = result_registration; + + if (result_registration.result != ResultRegistration::SUCCESS) { + RCLCPP_WARN(get_logger(), "Node registration failed for %s", node_name.c_str()); + return; + } + + response->uuid_node = node_uuid.value(); +} + +void ControlCenter::on_deregister_node( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto & result_deregistration = deregister_node(request->uuid_node); + response->result_deregistration.result = result_deregistration.result; + + if (result_deregistration.result != ResultDeregistration::SUCCESS) { + RCLCPP_WARN( + get_logger(), "Node deregistration failed for %s", + autoware_utils::to_hex_string(request->uuid_node).c_str()); + } +} + +} // namespace autoware::control_center diff --git a/common/autoware_control_center/test/config/control_center.param.yaml b/common/autoware_control_center/test/config/control_center.param.yaml new file mode 100644 index 00000000..fe6f644c --- /dev/null +++ b/common/autoware_control_center/test/config/control_center.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + deadline_ms: 220.0 + report_publish_rate: 1.0 diff --git a/common/autoware_control_center/test/test_cc_heartbeat.cpp b/common/autoware_control_center/test/test_cc_heartbeat.cpp new file mode 100644 index 00000000..8b111e83 --- /dev/null +++ b/common/autoware_control_center/test/test_cc_heartbeat.cpp @@ -0,0 +1,142 @@ +// 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 "test_utility.hpp" + +#include +#include +#include + +#include +#include + +#include + +namespace autoware::control_center +{ + +class ControlCenterHeartbeatTest : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + + node_options_.append_parameter_override("deadline_ms", deadline_ms_); + node_options_.append_parameter_override( + "report_publish_rate", 1000.0 / period_report_publish_ms_); + executor_ = std::make_shared(); + + control_center_ = std::make_shared(node_options_); + + executor_->add_node(control_center_->get_node_base_interface()); + + thread_spin_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + rclcpp::shutdown(); + if (thread_spin_.joinable()) { + thread_spin_.join(); + } + } + + 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_; + std::shared_ptr executor_; + ControlCenter::SharedPtr control_center_; + std::thread thread_spin_; + double deadline_ms_{500.0}; + double period_report_publish_ms_{10.0}; +}; + +TEST_F(ControlCenterHeartbeatTest, HeartbeatHandling) +{ + const auto [uuid, node] = test::register_node("node_test_heartbeat", ""); + + // Send a heartbeat and verify node status is updated + auto hb_healthy = test::generate_hb_healthy(); + auto pub_hb = test::send_first_heartbeat(node, deadline_ms_, hb_healthy); + + double sleep_duration_ms = deadline_ms_ * 0.8; + ASSERT_LT(period_report_publish_ms_, sleep_duration_ms); + + std::this_thread::sleep_for(std::chrono::duration(sleep_duration_ms)); + { + SCOPED_TRACE(""); + validate_heartbeat(node->get_fully_qualified_name(), hb_healthy); + } + + for (int i = 0; i < 3; ++i) { + hb_healthy = test::generate_hb_healthy(); + test::send_heartbeat(node, pub_hb, hb_healthy); + std::this_thread::sleep_for(std::chrono::duration(sleep_duration_ms)); + { + SCOPED_TRACE("validate_heartbeat iteration " + std::to_string(i)); + validate_heartbeat(node->get_fully_qualified_name(), hb_healthy); + } + } +} + +TEST_F(ControlCenterHeartbeatTest, HeartbeatMissedDeadline) +{ + const auto [uuid, node] = test::register_node("node_test_heartbeat", ""); + + // Send a heartbeat + auto pub_hb = test::send_first_heartbeat(node, deadline_ms_, test::generate_hb_healthy()); + + // Wait for the heartbeat deadline to be missed + std::this_thread::sleep_for( + std::chrono::duration(deadline_ms_ + period_report_publish_ms_ * 2.0)); + + // Expect the node to be marked as dead + autoware_control_center_msgs::msg::NodeReport 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); + ASSERT_EQ( + report.status_operational.status, + autoware_control_center_msgs::msg::NodeStatusOperational::UNKNOWN); + + // Send a heartbeat and verify node status is updated + auto hb_healthy = test::generate_hb_healthy(); + test::send_heartbeat(node, pub_hb, hb_healthy); + + // Wait shortly and expect the node to be alive + std::this_thread::sleep_for( + std::chrono::duration(period_report_publish_ms_ * 2.0)); + { + SCOPED_TRACE(""); + validate_heartbeat(node->get_fully_qualified_name(), hb_healthy); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +} // namespace autoware::control_center diff --git a/common/autoware_control_center/test/test_cc_init_shutdown.cpp b/common/autoware_control_center/test/test_cc_init_shutdown.cpp new file mode 100644 index 00000000..39d4665b --- /dev/null +++ b/common/autoware_control_center/test/test_cc_init_shutdown.cpp @@ -0,0 +1,59 @@ +// 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 +#include +#include + +#include + +#include + +class ControlCenterInitShutdownTest : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_options_.append_parameter_override("deadline_ms", 220.0); + node_options_.append_parameter_override("report_publish_rate", 1.0); + executor_ = std::make_shared(); + + control_center_ = std::make_shared(node_options_); + executor_->add_node(control_center_->get_node_base_interface()); + executor_->spin_some(std::chrono::milliseconds(50)); + } + + void TearDown() override { rclcpp::shutdown(); } + + rclcpp::NodeOptions node_options_; + autoware::control_center::ControlCenter::SharedPtr control_center_; + std::shared_ptr executor_; +}; + +TEST_F(ControlCenterInitShutdownTest, NodeInitShutdown) +{ + ASSERT_EQ( + control_center_->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + auto state = control_center_->shutdown(); + executor_->spin_some(std::chrono::milliseconds(50)); + ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/autoware_control_center/test/test_cc_registering.cpp b/common/autoware_control_center/test/test_cc_registering.cpp new file mode 100644 index 00000000..f3e4f5e0 --- /dev/null +++ b/common/autoware_control_center/test/test_cc_registering.cpp @@ -0,0 +1,133 @@ +// 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 "test_utility.hpp" + +#include +#include +#include + +#include +#include +#include + +#include + +namespace autoware::control_center +{ + +class ControlCenterRegisteringTest : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_options_.append_parameter_override("deadline_ms", 220.0); + node_options_.append_parameter_override("report_publish_rate", 100.0); + executor_ = std::make_shared(); + + control_center_ = std::make_shared(node_options_); + + executor_->add_node(control_center_->get_node_base_interface()); + + thread_spin_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + rclcpp::shutdown(); + if (thread_spin_.joinable()) { + thread_spin_.join(); + } + } + + rclcpp::NodeOptions node_options_; + std::shared_ptr executor_; + ControlCenter::SharedPtr control_center_; + std::thread thread_spin_; +}; + +TEST_F(ControlCenterRegisteringTest, NodeRegistration) +{ + const std::string node_name = "node_test"; + const std::string ns; + const auto [uuid, node] = test::register_node(node_name, ns); + + // Make sure node is alive + ASSERT_EQ( + control_center_->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + // 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->get_fully_qualified_name(), report)); + ASSERT_EQ(report.uuid, uuid); + ASSERT_EQ(report.count_registered, 1); + ASSERT_TRUE(report.is_alive); + ASSERT_EQ( + report.status_activity.status, + autoware_control_center_msgs::msg::NodeStatusActivity::INITIALIZING); + ASSERT_EQ( + report.status_operational.status, + autoware_control_center_msgs::msg::NodeStatusOperational::WARNING); +} + +TEST_F(ControlCenterRegisteringTest, NodeDuplicateRegistration) +{ + const std::string node_name = "node_test_duplicate"; + const std::string ns; + const auto [uuid1, node1] = 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(node1->get_fully_qualified_name(), report)); + ASSERT_EQ(report.count_registered, 2); + ASSERT_TRUE(report.is_alive); + ASSERT_EQ( + report.status_activity.status, + autoware_control_center_msgs::msg::NodeStatusActivity::INITIALIZING); + ASSERT_EQ( + report.status_operational.status, + autoware_control_center_msgs::msg::NodeStatusOperational::WARNING); +} + +TEST_F(ControlCenterRegisteringTest, NodeDeregistration) +{ + const std::string node_name = "node_test_deregister"; + const std::string 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->get_fully_qualified_name())); +} + +TEST_F(ControlCenterRegisteringTest, DeregisterNonExistentNode) +{ + unique_identifier_msgs::msg::UUID uuid = autoware_utils::generate_uuid(); + ASSERT_FALSE(test::deregister_node(uuid)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +} // namespace autoware::control_center diff --git a/common/autoware_control_center/test/test_constants.hpp b/common/autoware_control_center/test/test_constants.hpp new file mode 100644 index 00000000..462c00c7 --- /dev/null +++ b/common/autoware_control_center/test/test_constants.hpp @@ -0,0 +1,26 @@ +// 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. + +#ifndef TEST_CONSTANTS_HPP_ +#define TEST_CONSTANTS_HPP_ + +namespace autoware::control_center::test +{ +constexpr char const * topic_register_service = "/autoware/control_center/srv/register"; +constexpr char const * topic_deregister_service = "/autoware/control_center/srv/deregister"; +constexpr char const * topic_node_reports = "/autoware/control_center/node_reports"; +constexpr char const * topic_heartbeat_suffix = "/heartbeat"; +} // namespace autoware::control_center::test + +#endif // TEST_CONSTANTS_HPP_ diff --git a/common/autoware_control_center/test/test_utility.hpp b/common/autoware_control_center/test/test_utility.hpp new file mode 100644 index 00000000..babe14f1 --- /dev/null +++ b/common/autoware_control_center/test/test_utility.hpp @@ -0,0 +1,173 @@ +// 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. + +#ifndef TEST_UTILITY_HPP_ +#define TEST_UTILITY_HPP_ + +#include "test_constants.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::control_center::test +{ + +inline std::tuple register_node( + const std::string & node_name, const std::string & ns) +{ + auto test_node = std::make_shared(node_name, ns); + + auto client = + test_node->create_client(topic_register_service); + auto request = std::make_shared(); + request->node_name_with_namespace = test_node->get_fully_qualified_name(); + + RCLCPP_INFO(rclcpp::get_logger("registering"), "Initializing service"); + if (!client->wait_for_service(std::chrono::seconds(5))) { + throw std::runtime_error("Service initialization timeout"); + } + + RCLCPP_INFO(rclcpp::get_logger("registering"), "Waiting for response"); + auto fut_result = client->async_send_request(request); + if ( + rclcpp::spin_until_future_complete(test_node, fut_result, std::chrono::seconds(5)) != + rclcpp::FutureReturnCode::SUCCESS) { + throw std::runtime_error("Service call timeout or failed"); + } + + auto result = fut_result.get(); + if ( + result->result_registration.result != + autoware_control_center_msgs::msg::ResultRegistration::SUCCESS) { + throw std::runtime_error("Node registration failed"); + } + RCLCPP_INFO(rclcpp::get_logger("registering"), "Received the response"); + + return {result->uuid_node, test_node}; +} + +inline bool deregister_node(const unique_identifier_msgs::msg::UUID & uuid) +{ + auto test_node = std::make_shared("test_node"); + + auto client = test_node->create_client( + topic_deregister_service); + auto request = std::make_shared(); + request->uuid_node = uuid; + + if (!client->wait_for_service(std::chrono::seconds(5))) { + return false; + } + + auto fut_result = client->async_send_request(request); + if ( + rclcpp::spin_until_future_complete(test_node, fut_result, std::chrono::seconds(5)) != + rclcpp::FutureReturnCode::SUCCESS) { + return false; + } + + auto result = fut_result.get(); + return result->result_deregistration.result == + autoware_control_center_msgs::msg::ResultDeregistration::SUCCESS; +} + +inline bool wait_for_node_report( + const std::string & node_full_name, autoware_control_center_msgs::msg::NodeReport & report, + const long timeout_ms = 5000) +{ + std::promise report_received; + auto future_report = report_received.get_future(); + + auto node_temp = std::make_shared("node_report_listener", ""); + + auto node_reports_sub = + node_temp->create_subscription( + topic_node_reports, rclcpp::QoS(rclcpp::KeepLast(0)), + [&report_received, &report, + &node_full_name](const autoware_control_center_msgs::msg::NodeReports::SharedPtr msg) { + for (const auto & node_report : msg->reports) { + if (node_report.name == node_full_name) { + report = node_report; + report_received.set_value(true); + return; + } + } + report_received.set_value(false); + }); + + if ( + rclcpp::spin_until_future_complete( + node_temp, future_report, std::chrono::milliseconds(timeout_ms)) != + rclcpp::FutureReturnCode::SUCCESS) { + return false; + } + return future_report.get(); +} + +inline bool is_node_registered(const std::string & node_full_name) +{ + autoware_control_center_msgs::msg::NodeReport report; + return wait_for_node_report(node_full_name, report); +} + +inline void send_heartbeat( + const rclcpp::Node::SharedPtr & node, + const rclcpp::Publisher::SharedPtr & pub, + const autoware_control_center_msgs::msg::Heartbeat::SharedPtr & heartbeat) +{ + pub->publish(*heartbeat); + rclcpp::spin_some(node); +} + +inline rclcpp::Publisher::SharedPtr +send_first_heartbeat( + const rclcpp::Node::SharedPtr & node, const double deadline_ms, + const autoware_control_center_msgs::msg::Heartbeat::SharedPtr & heartbeat) +{ + 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 = node->create_publisher( + fully_qualified_name + test::topic_heartbeat_suffix, qos_profile); + send_heartbeat(node, pub, heartbeat); + return pub; +} + +autoware_control_center_msgs::msg::Heartbeat::SharedPtr generate_hb_healthy() +{ + auto heartbeat = std::make_shared(); + 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; + return heartbeat; +} +} // namespace autoware::control_center::test + +#endif // TEST_UTILITY_HPP_ diff --git a/common/autoware_control_center_msgs/CMakeLists.txt b/common/autoware_control_center_msgs/CMakeLists.txt new file mode 100644 index 00000000..cba625fc --- /dev/null +++ b/common/autoware_control_center_msgs/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_control_center_msgs) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +set(msg_dependencies + builtin_interfaces + unique_identifier_msgs) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Heartbeat.msg" + "msg/NodeReport.msg" + "msg/NodeReports.msg" + "msg/NodeStatusActivity.msg" + "msg/NodeStatusOperational.msg" + "msg/ResultDeregistration.msg" + "msg/ResultRegistration.msg" + "srv/Deregister.srv" + "srv/Register.srv" + DEPENDENCIES ${msg_dependencies}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/common/autoware_control_center_msgs/README.md b/common/autoware_control_center_msgs/README.md new file mode 100644 index 00000000..8bfa7c3f --- /dev/null +++ b/common/autoware_control_center_msgs/README.md @@ -0,0 +1,3 @@ +# autoware_msgs + +Before contributing, review [the message guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/message-guidelines/). diff --git a/common/autoware_control_center_msgs/msg/Heartbeat.msg b/common/autoware_control_center_msgs/msg/Heartbeat.msg new file mode 100644 index 00000000..5d241592 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/Heartbeat.msg @@ -0,0 +1,17 @@ +# Heartbeat message designed to monitor node liveliness with an additional +# sequence number to track message order and detect potential drops. + +# Stamp when the message was generated +builtin_interfaces/Time stamp + +# Node identifier +unique_identifier_msgs/UUID uuid_node + +# Incremental sequence number used to verify the order of received heartbeats +# and identify potential message drops or delays. +# This number will wrap around once it reaches to its maximum value (65535) +uint16 sequence_number + +# Status of the node +autoware_control_center_msgs/NodeStatusActivity status_activity +autoware_control_center_msgs/NodeStatusOperational status_operational diff --git a/common/autoware_control_center_msgs/msg/NodeReport.msg b/common/autoware_control_center_msgs/msg/NodeReport.msg new file mode 100644 index 00000000..5c9d076e --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeReport.msg @@ -0,0 +1,15 @@ +# Periodic report from the Autoware Control Center on the status of a registered node. + +# Node information +string name +unique_identifier_msgs/UUID uuid + +# Registration information +builtin_interfaces/Time stamp_registration +uint16 count_registered + +# Status of the node +autoware_control_center_msgs/NodeStatusActivity status_activity +autoware_control_center_msgs/NodeStatusOperational status_operational +builtin_interfaces/Time stamp_last_heartbeat +bool is_alive diff --git a/common/autoware_control_center_msgs/msg/NodeReports.msg b/common/autoware_control_center_msgs/msg/NodeReports.msg new file mode 100644 index 00000000..e878c43c --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeReports.msg @@ -0,0 +1,4 @@ +# Collection of periodic reports from the Autoware Control Center on the status of multiple registered nodes. + +builtin_interfaces/Time stamp +NodeReport[] reports diff --git a/common/autoware_control_center_msgs/msg/NodeStatusActivity.msg b/common/autoware_control_center_msgs/msg/NodeStatusActivity.msg new file mode 100644 index 00000000..0a99b984 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeStatusActivity.msg @@ -0,0 +1,10 @@ +# Enumeration of possible activity statuses for Autoware nodes. +uint8 UNKNOWN=0 # Activity status unknown +uint8 INITIALIZING=1 # Node is initializing +uint8 IDLE=2 # Node is idle +uint8 PROCESSING=3 # Node is actively processing +uint8 WAITING_FOR_MESSAGE=4 # Node is waiting for a message +uint8 SHUTTING_DOWN=5 # Node is shutting down + +# Current activity status +uint8 status diff --git a/common/autoware_control_center_msgs/msg/NodeStatusOperational.msg b/common/autoware_control_center_msgs/msg/NodeStatusOperational.msg new file mode 100644 index 00000000..5de0e17f --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeStatusOperational.msg @@ -0,0 +1,9 @@ +# Enumeration of possible operational statuses for Autoware nodes. +uint8 UNKNOWN=0 # Operational status unknown +uint8 NORMAL=1 # Node is operating normally +uint8 WARNING=2 # Node is in a warning status +uint8 ERROR=3 # Node has encountered an error +uint8 FATAL=4 # Node is in a fatal status + +# Current operational status +uint8 status diff --git a/common/autoware_control_center_msgs/msg/ResultDeregistration.msg b/common/autoware_control_center_msgs/msg/ResultDeregistration.msg new file mode 100644 index 00000000..9a995e74 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/ResultDeregistration.msg @@ -0,0 +1,7 @@ +# Enumeration of possible deregistration outcomes. +uint8 SUCCESS=0 # Deregistration was successful +uint8 FAILURE_OTHER=1 # Other unspecified reason +uint8 FAILURE_NOT_REGISTERED=2 # Node is not registered +uint8 FAILURE_INVALID_NODE=3 # Node information is invalid + +uint8 result diff --git a/common/autoware_control_center_msgs/msg/ResultRegistration.msg b/common/autoware_control_center_msgs/msg/ResultRegistration.msg new file mode 100644 index 00000000..62fd1b24 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/ResultRegistration.msg @@ -0,0 +1,6 @@ +# Enumeration of possible registration outcomes. +uint8 SUCCESS=0 # Registration was successful +uint8 FAILURE_OTHER=1 # Other unspecified reason +uint8 FAILURE_INVALID_NODE=2 # Node information is invalid + +uint8 result diff --git a/common/autoware_control_center_msgs/package.xml b/common/autoware_control_center_msgs/package.xml new file mode 100644 index 00000000..f01bebee --- /dev/null +++ b/common/autoware_control_center_msgs/package.xml @@ -0,0 +1,26 @@ + + + + autoware_control_center_msgs + 1.0.0 + autoware_control_center_msgs package. + M. Fatih Cırıt + Apache License 2.0 + + ament_cmake_auto + + rosidl_default_generators + + builtin_interfaces + unique_identifier_msgs + + rosidl_default_runtime + + autoware_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/common/autoware_control_center_msgs/srv/Deregister.srv b/common/autoware_control_center_msgs/srv/Deregister.srv new file mode 100644 index 00000000..3f74b94a --- /dev/null +++ b/common/autoware_control_center_msgs/srv/Deregister.srv @@ -0,0 +1,3 @@ +unique_identifier_msgs/UUID uuid_node +--- +autoware_control_center_msgs/ResultDeregistration result_deregistration diff --git a/common/autoware_control_center_msgs/srv/Register.srv b/common/autoware_control_center_msgs/srv/Register.srv new file mode 100644 index 00000000..80281d0f --- /dev/null +++ b/common/autoware_control_center_msgs/srv/Register.srv @@ -0,0 +1,4 @@ +string node_name_with_namespace +--- +autoware_control_center_msgs/ResultRegistration result_registration +unique_identifier_msgs/UUID uuid_node diff --git a/common/autoware_node/CMakeLists.txt b/common/autoware_node/CMakeLists.txt new file mode 100644 index 00000000..ac7d9f2a --- /dev/null +++ b/common/autoware_node/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_node) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} src/node.cpp) + +if(BUILD_TESTING) + file(GLOB_RECURSE TEST_FILES test/*.cpp) + + foreach(TEST_FILE ${TEST_FILES}) + # Get the test name without directory and extension + get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) + + # Add each test separately + ament_add_ros_isolated_gtest(${TEST_NAME} ${TEST_FILE} TIMEOUT 10) + target_include_directories(${TEST_NAME} PRIVATE src/include) + target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) + ament_target_dependencies(${TEST_NAME} + rclcpp + rclcpp_lifecycle + autoware_utils + autoware_control_center_msgs + autoware_control_center) + endforeach() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config) diff --git a/common/autoware_node/README.md b/common/autoware_node/README.md new file mode 100644 index 00000000..6b03a4d5 --- /dev/null +++ b/common/autoware_node/README.md @@ -0,0 +1,72 @@ +# Autoware Node + +## Abbreviations + +- **ACC:** Autoware Control Center +- **AN:** Autoware Node +- **HB:** Heartbeat + +## Overview + +AN is an `autoware.core` package designed to provide a base class for all future nodes in the +system. +It provides ability to register node to ACC, report node state, publish heartbeats. +It also inherits all lifecycle control capabilities of the base +class [LifecycleNode](https://docs.ros2.org/latest/api/rclcpp_lifecycle/classrclcpp__lifecycle_1_1LifecycleNode.html) + +## Usage + +Check the [autoware_test_node](../autoware_test_node/README.md) package for an example of how to use `autoware::Node`. + +## Design + +### Lifecycle + +AN inherits from ROS 2 [rclcpp_lifecycle::LifecycleNode](https://design.ros2.org/articles/node_lifecycle.html) and has +all the basic functions of it. + +### Registration + +Relevant parameters: + +| Parameter | Function | +| -------------------------- | ---------------------------------------------------------------------- | +| `period_timer_register_ms` | The interval at which the AN periodically attempts to register itself. | + +Upon startup, AN starts a registration timer and periodically attempts to register itself to ACC via a service call. +This registration timer runs asynchronously to the rest of the node's functionality. + +Upon successful registration: + +- The registration timer is stopped. +- ACC will create a subscription to the HB messages of AN. +- ACC will return a UUID specific to the AN instance. + +### Error state + +WIP. +Currently, only sends positive heartbeat messages. + +### Heartbeat (**HB**) + +Relevant parameters: + +| Parameter | Function | +| --------------------- | ---------------------------------------------------------------------- | +| `deadline_ms` | If ACC doesn't receive a HB by this deadline, AN will be assumed dead. | +| `period_heartbeat_ms` | AN is expected to publish a HB with this period. | + +- 🟡 `deadline_ms` should be slightly higher than `period_heartbeat_ms` to account for network delays. +- 🔴 `deadline_ms` of AN should match the `deadline_ms` of ACC. + +Upon registration, AN starts a timer that periodically publishes HB messages to ACC. +This HB timer runs asynchronously to the rest of the node's functionality. + +### Monitored subscription + +WIP. + +## Credits + +- Heartbeat functionality is based on ROS 2 [software_watchdogs](https://github.com/ros-safety/software_watchdogs) + package. diff --git a/common/autoware_node/config/node.param.yaml b/common/autoware_node/config/node.param.yaml new file mode 100644 index 00000000..6d55f628 --- /dev/null +++ b/common/autoware_node/config/node.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + period_timer_register_ms: 100.0 + period_heartbeat_ms: 200.0 + deadline_ms: 220.0 diff --git a/common/autoware_node/include/autoware/node/node.hpp b/common/autoware_node/include/autoware/node/node.hpp new file mode 100644 index 00000000..c085bb45 --- /dev/null +++ b/common/autoware_node/include/autoware/node/node.hpp @@ -0,0 +1,88 @@ +// 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. + +#ifndef AUTOWARE__NODE__NODE_HPP_ +#define AUTOWARE__NODE__NODE_HPP_ + +#include "autoware/node/visibility_control.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::node +{ +class Node : public rclcpp_lifecycle::LifecycleNode +{ +public: + AUTOWARE_NODE_PUBLIC + explicit Node( + const std::string & node_name, const std::string & ns = "", + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + using Heartbeat = autoware_control_center_msgs::msg::Heartbeat; + using NodeStatusActivity = autoware_control_center_msgs::msg::NodeStatusActivity; + using NodeStatusOperational = autoware_control_center_msgs::msg::NodeStatusOperational; + using ResultDeregistration = autoware_control_center_msgs::msg::ResultDeregistration; + using ResultRegistration = autoware_control_center_msgs::msg::ResultRegistration; + using Deregister = autoware_control_center_msgs::srv::Deregister; + using Register = autoware_control_center_msgs::srv::Register; + + using FutureRegister = rclcpp::Client::SharedFuture; + + rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_; + + rclcpp::Client::SharedPtr cli_register_; + + rclcpp::Publisher::SharedPtr pub_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_; + + void on_tick_registration(); + void on_tick_heartbeat(); + void on_register(FutureRegister future); + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & state) override; + +protected: + void destroy_node(); +}; +} // namespace autoware::node + +#endif // AUTOWARE__NODE__NODE_HPP_ diff --git a/common/autoware_node/include/autoware/node/visibility_control.hpp b/common/autoware_node/include/autoware/node/visibility_control.hpp new file mode 100644 index 00000000..b7a6bbd0 --- /dev/null +++ b/common/autoware_node/include/autoware/node/visibility_control.hpp @@ -0,0 +1,26 @@ +// 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. + +#ifndef AUTOWARE__NODE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__NODE__VISIBILITY_CONTROL_HPP_ + +#include "rcutils/visibility_control_macros.h" +#ifdef AUTOWARE_NODE_BUILDING_DLL +#define AUTOWARE_NODE_PUBLIC RCUTILS_EXPORT +#else +#define AUTOWARE_NODE_PUBLIC RCUTILS_IMPORT +#endif // !AUTOWARE_NODE_BUILDING_DLL +#define AUTOWARE_NODE_LOCAL RCUTILS_LOCAL + +#endif // AUTOWARE__NODE__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_node/package.xml b/common/autoware_node/package.xml new file mode 100644 index 00000000..96aed8ee --- /dev/null +++ b/common/autoware_node/package.xml @@ -0,0 +1,24 @@ + + + + autoware_node + 0.0.0 + Autoware Node is an Autoware.Core package designed to provide a base class for all nodes in the system. + M. Fatih Cırıt + Apache-2.0 + + ament_cmake_auto + autoware_cmake + + autoware_control_center_msgs + autoware_utils + rclcpp_lifecycle + + ament_cmake_ros + autoware_control_center + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_node/src/node.cpp b/common/autoware_node/src/node.cpp new file mode 100644 index 00000000..4add8489 --- /dev/null +++ b/common/autoware_node/src/node.cpp @@ -0,0 +1,126 @@ +// 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/node/node.hpp" + +#include +#include + +#include +#include + +#include + +namespace autoware::node +{ +Node::Node( + const std::string & node_name, const std::string & ns, const rclcpp::NodeOptions & options) +: LifecycleNode(node_name, ns, options), + sequence_number_{0}, + is_registered_{false}, + period_timer_register_ms_{declare_parameter("period_timer_register_ms")}, + period_heartbeat_ms_{declare_parameter("period_heartbeat_ms")}, + deadline_ms_{declare_parameter("deadline_ms")} +{ + RCLCPP_DEBUG( + get_logger(), "Node %s constructor", get_node_base_interface()->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_)); + + pub_heartbeat_ = this->create_publisher( + "~/heartbeat", qos_profile); + + callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + timer_heartbeat_ = this->create_wall_timer( + std::chrono::duration(period_heartbeat_ms_), + std::bind(&Node::on_tick_heartbeat, this), callback_group_mut_ex_); + + cli_register_ = create_client( + "/autoware/control_center/srv/register", rmw_qos_profile_default, callback_group_mut_ex_); + + timer_registration_ = this->create_wall_timer( + std::chrono::duration(period_timer_register_ms_), + std::bind(&Node::on_tick_registration, this), callback_group_mut_ex_); +} + +void Node::on_tick_heartbeat() +{ + auto heartbeat = std::make_shared(); + 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() +{ + RCLCPP_DEBUG(get_logger(), "on_tick_registration"); + if (is_registered_) { + RCLCPP_DEBUG(get_logger(), "Already registered."); + return; + } + + if (!cli_register_->service_is_ready()) { + RCLCPP_WARN(get_logger(), "%s is unavailable.", cli_register_->get_service_name()); + return; + } + autoware_control_center_msgs::srv::Register::Request::SharedPtr req = + std::make_shared(); + req->node_name_with_namespace = get_node_base_interface()->get_fully_qualified_name(); + + cli_register_->async_send_request( + req, std::bind(&Node::on_register, this, std::placeholders::_1)); + RCLCPP_DEBUG(get_logger(), "Sent registration request."); +} + +void Node::on_register(FutureRegister future) +{ + const auto & response = future.get(); + + if ( + response->result_registration.result != + autoware_control_center_msgs::msg::ResultRegistration::SUCCESS) { + is_registered_ = false; + RCLCPP_WARN(get_logger(), "Registration failed."); + return; + } + + uuid_node_ = response->uuid_node; + is_registered_ = true; + RCLCPP_DEBUG(get_logger(), "Registration succeeded."); +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Node::on_shutdown( + const rclcpp_lifecycle::State & state) +{ + destroy_node(); + return LifecycleNode::on_shutdown(state); +} + +void Node::destroy_node() +{ + timer_registration_.reset(); + timer_heartbeat_.reset(); + cli_register_.reset(); + pub_heartbeat_.reset(); +} +} // namespace autoware::node diff --git a/common/autoware_node/test/test_an_heartbeat.cpp b/common/autoware_node/test/test_an_heartbeat.cpp new file mode 100644 index 00000000..e348c827 --- /dev/null +++ b/common/autoware_node/test/test_an_heartbeat.cpp @@ -0,0 +1,104 @@ +// 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 "test_utility.hpp" + +#include +#include +#include +#include + +#include + +#include + +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 executor; + executor = std::make_shared(); + + autoware::node::Node::SharedPtr autoware_node = + std::make_shared("test_node", "test_ns", node_options_an_); + autoware::node::Node::SharedPtr control_center = + std::make_shared(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(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(period_heartbeat_ms_)); + } + } + + executor->cancel(); // make sure cancel is called after spin + if (thread_spin.joinable()) { + thread_spin.join(); + } +} +} // namespace autoware::node diff --git a/common/autoware_node/test/test_an_init_shutdown.cpp b/common/autoware_node/test/test_an_init_shutdown.cpp new file mode 100644 index 00000000..715caa9d --- /dev/null +++ b/common/autoware_node/test/test_an_init_shutdown.cpp @@ -0,0 +1,77 @@ +// 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 +#include +#include + +#include + +#include + +class AutowareNodeInitShutdown : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + 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", deadline_ms); + node_options_acc_.append_parameter_override("report_publish_rate", 100.0); + } + + void TearDown() override { rclcpp::shutdown(); } + + rclcpp::NodeOptions node_options_an_; + rclcpp::NodeOptions node_options_acc_; +}; + +TEST_F(AutowareNodeInitShutdown, NodeInitShutdown) +{ + std::shared_ptr executor; + executor = std::make_shared(); + + autoware::node::Node::SharedPtr autoware_node = + std::make_shared("test_node", "test_ns", node_options_an_); + autoware::node::Node::SharedPtr control_center = + std::make_shared(node_options_acc_); + + executor->add_node(control_center->get_node_base_interface()); + executor->add_node(autoware_node->get_node_base_interface()); + + std::thread thread_spin = std::thread([&executor]() { executor->spin(); }); + + ASSERT_EQ( + autoware_node->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto state = autoware_node->shutdown(); + + ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + // wait until executor is spinning + while (!executor->is_spinning()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + executor->cancel(); // make sure cancel is called after spin + if (thread_spin.joinable()) { + thread_spin.join(); + } +} + +// TEST_F(AutowareNodeTest, Sen) diff --git a/common/autoware_node/test/test_an_registering.cpp b/common/autoware_node/test/test_an_registering.cpp new file mode 100644 index 00000000..4fddc1fb --- /dev/null +++ b/common/autoware_node/test/test_an_registering.cpp @@ -0,0 +1,129 @@ +// 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 "test_utility.hpp" + +#include +#include +#include +#include + +#include + +#include + +namespace autoware::node +{ +class AutowareNodeRegistering : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + 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", deadline_ms); + node_options_acc_.append_parameter_override("report_publish_rate", 100.0); + } + + void TearDown() override { rclcpp::shutdown(); } + + rclcpp::NodeOptions node_options_an_; + rclcpp::NodeOptions node_options_acc_; +}; + +TEST_F(AutowareNodeRegistering, RegisterSuccessFirstACCThenAN) +{ + std::shared_ptr executor; + executor = std::make_shared(); + + autoware::node::Node::SharedPtr autoware_node = + std::make_shared("test_node", "test_ns", node_options_an_); + autoware::node::Node::SharedPtr control_center = + std::make_shared(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 + 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(AutowareNodeRegistering, RegisterSuccessFirstANThenACC) +{ + std::shared_ptr executor; + executor = std::make_shared(); + + autoware::node::Node::SharedPtr autoware_node = + std::make_shared("test_node", "test_ns", node_options_an_); + autoware::node::Node::SharedPtr control_center = + std::make_shared(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)); + } + + // 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(); + } +} +} // namespace autoware::node diff --git a/common/autoware_node/test/test_constants.hpp b/common/autoware_node/test/test_constants.hpp new file mode 100644 index 00000000..bca02228 --- /dev/null +++ b/common/autoware_node/test/test_constants.hpp @@ -0,0 +1,26 @@ +// 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. + +#ifndef TEST_CONSTANTS_HPP_ +#define TEST_CONSTANTS_HPP_ + +namespace autoware::node::test +{ +constexpr char const * topic_register_service = "/autoware/control_center/srv/register"; +constexpr char const * topic_deregister_service = "/autoware/control_center/srv/deregister"; +constexpr char const * topic_node_reports = "/autoware/control_center/node_reports"; +constexpr char const * topic_heartbeat_suffix = "/heartbeat"; +} // namespace autoware::node::test + +#endif // TEST_CONSTANTS_HPP_ diff --git a/common/autoware_node/test/test_utility.hpp b/common/autoware_node/test/test_utility.hpp new file mode 100644 index 00000000..fec99f5b --- /dev/null +++ b/common/autoware_node/test/test_utility.hpp @@ -0,0 +1,173 @@ +// 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. + +#ifndef TEST_UTILITY_HPP_ +#define TEST_UTILITY_HPP_ + +#include "test_constants.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::node::test +{ + +inline std::tuple register_node( + const std::string & node_name, const std::string & ns) +{ + auto test_node = std::make_shared(node_name, ns); + + auto client = + test_node->create_client(topic_register_service); + auto request = std::make_shared(); + request->node_name_with_namespace = test_node->get_fully_qualified_name(); + + RCLCPP_INFO(rclcpp::get_logger("registering"), "Initializing service"); + if (!client->wait_for_service(std::chrono::seconds(5))) { + throw std::runtime_error("Service initialization timeout"); + } + + RCLCPP_INFO(rclcpp::get_logger("registering"), "Waiting for response"); + auto fut_result = client->async_send_request(request); + if ( + rclcpp::spin_until_future_complete(test_node, fut_result, std::chrono::seconds(5)) != + rclcpp::FutureReturnCode::SUCCESS) { + throw std::runtime_error("Service call timeout or failed"); + } + + auto result = fut_result.get(); + if ( + result->result_registration.result != + autoware_control_center_msgs::msg::ResultRegistration::SUCCESS) { + throw std::runtime_error("Node registration failed"); + } + RCLCPP_INFO(rclcpp::get_logger("registering"), "Received the response"); + + return {result->uuid_node, test_node}; +} + +inline bool deregister_node(const unique_identifier_msgs::msg::UUID & uuid) +{ + auto test_node = std::make_shared("test_node"); + + auto client = test_node->create_client( + topic_deregister_service); + auto request = std::make_shared(); + request->uuid_node = uuid; + + if (!client->wait_for_service(std::chrono::seconds(5))) { + return false; + } + + auto fut_result = client->async_send_request(request); + if ( + rclcpp::spin_until_future_complete(test_node, fut_result, std::chrono::seconds(5)) != + rclcpp::FutureReturnCode::SUCCESS) { + return false; + } + + auto result = fut_result.get(); + return result->result_deregistration.result == + autoware_control_center_msgs::msg::ResultDeregistration::SUCCESS; +} + +inline bool wait_for_node_report( + const std::string & node_full_name, autoware_control_center_msgs::msg::NodeReport & report, + const long timeout_ms = 5000) +{ + std::promise report_received; + auto future_report = report_received.get_future(); + + auto node_temp = std::make_shared("node_report_listener", ""); + + auto node_reports_sub = + node_temp->create_subscription( + topic_node_reports, rclcpp::QoS(rclcpp::KeepLast(0)), + [&report_received, &report, + &node_full_name](const autoware_control_center_msgs::msg::NodeReports::SharedPtr msg) { + for (const auto & node_report : msg->reports) { + if (node_report.name == node_full_name) { + report = node_report; + report_received.set_value(true); + return; + } + } + report_received.set_value(false); + }); + + if ( + rclcpp::spin_until_future_complete( + node_temp, future_report, std::chrono::milliseconds(timeout_ms)) != + rclcpp::FutureReturnCode::SUCCESS) { + return false; + } + return future_report.get(); +} + +inline bool is_node_registered(const std::string & node_full_name) +{ + autoware_control_center_msgs::msg::NodeReport report; + return wait_for_node_report(node_full_name, report); +} + +inline void send_heartbeat( + const rclcpp::Node::SharedPtr & node, + const rclcpp::Publisher::SharedPtr & pub, + const autoware_control_center_msgs::msg::Heartbeat::SharedPtr & heartbeat) +{ + pub->publish(*heartbeat); + rclcpp::spin_some(node); +} + +inline rclcpp::Publisher::SharedPtr +send_first_heartbeat( + const rclcpp::Node::SharedPtr & node, const double deadline_ms, + const autoware_control_center_msgs::msg::Heartbeat::SharedPtr & heartbeat) +{ + 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 = node->create_publisher( + fully_qualified_name + test::topic_heartbeat_suffix, qos_profile); + send_heartbeat(node, pub, heartbeat); + return pub; +} + +autoware_control_center_msgs::msg::Heartbeat::SharedPtr generate_hb_healthy() +{ + auto heartbeat = std::make_shared(); + 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; + return heartbeat; +} +} // namespace autoware::node::test + +#endif // TEST_UTILITY_HPP_ diff --git a/common/autoware_test_node/CMakeLists.txt b/common/autoware_test_node/CMakeLists.txt new file mode 100644 index 00000000..64db7b3b --- /dev/null +++ b/common/autoware_test_node/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_test_node) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/test_node.cpp) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::test_node::TestNode" + EXECUTABLE ${PROJECT_NAME}_node) + +ament_auto_package(INSTALL_TO_SHARE + launch) diff --git a/common/autoware_test_node/README.md b/common/autoware_test_node/README.md new file mode 100644 index 00000000..e81c7dfb --- /dev/null +++ b/common/autoware_test_node/README.md @@ -0,0 +1,58 @@ +# autoware_test_node + +This package contains a simple example of how to use `autoware::Node`. + +## Usage + +```bash +ros2 launch autoware_test_node autoware_node_with_cc.launch.xml +``` + +On another terminal: + +```bash +ros2 topic echo /autoware/control_center/node_reports +``` + +You should see the reports coming from the nodes. + +### Lifecycle control + +Output a list of nodes with lifecycle: + +```console +$ ros2 lifecycle nodes +/autoware/control_center +/test_node3 +/test_node4 +/test_ns1/test_node1 +/test_ns2/test_node2 +``` + +Get the current state of a node: + +```console +$ ros2 lifecycle get /test_node4 +unconfigured [1] +``` + +List the available transitions for the node: + +```console +$ ros2 lifecycle list /test_node4 +- configure [1] + Start: unconfigured + Goal: configuring +- shutdown [5] + Start: unconfigured + Goal: shuttingdown +``` + +Shutdown the node: + +```console +$ ros2 lifecycle set /test_node4 shutdown +Transitioning successful +``` + +Check the `/autoware/control_center/node_reports` topic to see the node's `is_alive` field change to `false`. diff --git a/common/autoware_test_node/launch/autoware_node_with_cc.launch.xml b/common/autoware_test_node/launch/autoware_node_with_cc.launch.xml new file mode 100644 index 00000000..b83bfcb0 --- /dev/null +++ b/common/autoware_test_node/launch/autoware_node_with_cc.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_test_node/package.xml b/common/autoware_test_node/package.xml new file mode 100644 index 00000000..d79a8c13 --- /dev/null +++ b/common/autoware_test_node/package.xml @@ -0,0 +1,27 @@ + + + + autoware_test_node + 0.0.0 + Test package for Autoware Node. + M. Fatih Cırıt + Apache-2.0 + + ament_cmake_auto + autoware_cmake + + autoware_control_center_msgs + autoware_node + rclcpp + rclcpp_components + rclcpp_lifecycle + std_msgs + + ament_cmake_ros + autoware_control_center + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_test_node/src/include/test_node.hpp b/common/autoware_test_node/src/include/test_node.hpp new file mode 100644 index 00000000..027171d2 --- /dev/null +++ b/common/autoware_test_node/src/include/test_node.hpp @@ -0,0 +1,32 @@ +// 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. + +#ifndef TEST_NODE_HPP_ +#define TEST_NODE_HPP_ + +#include +#include + +namespace autoware::test_node +{ + +class TestNode : public autoware::node::Node +{ +public: + explicit TestNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); +}; + +} // namespace autoware::test_node + +#endif // TEST_NODE_HPP_ diff --git a/common/autoware_test_node/src/test_node.cpp b/common/autoware_test_node/src/test_node.cpp new file mode 100644 index 00000000..b67ddaff --- /dev/null +++ b/common/autoware_test_node/src/test_node.cpp @@ -0,0 +1,31 @@ +// 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 "include/test_node.hpp" + +#include + +namespace autoware::test_node +{ +TestNode::TestNode(const rclcpp::NodeOptions & options) +: autoware::node::Node("test_node", "", options) +{ + RCLCPP_INFO( + get_logger(), "TestNode constructor with name %s", + this->get_node_base_interface()->get_fully_qualified_name()); +} +} // namespace autoware::test_node + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::test_node::TestNode) diff --git a/common/autoware_test_node/test/test_test_node.cpp b/common/autoware_test_node/test/test_test_node.cpp new file mode 100644 index 00000000..b7b9079a --- /dev/null +++ b/common/autoware_test_node/test/test_test_node.cpp @@ -0,0 +1,15 @@ +// 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 "test_test_node.hpp" diff --git a/common/autoware_test_node/test/test_test_node.hpp b/common/autoware_test_node/test/test_test_node.hpp new file mode 100644 index 00000000..3fa5f1ef --- /dev/null +++ b/common/autoware_test_node/test/test_test_node.hpp @@ -0,0 +1,18 @@ +// 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. + +#ifndef TEST_TEST_NODE_HPP_ +#define TEST_TEST_NODE_HPP_ + +#endif // TEST_TEST_NODE_HPP_