From a1b7090c622a4b1bba9532395746b2bbfd9873a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Fri, 19 Jul 2024 14:41:21 +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 | 61 +++++ common/autoware_control_center/README.md | 99 +++++++ .../config/control_center.param.yaml | 4 + .../launch/control_center.launch.xml | 8 + common/autoware_control_center/package.xml | 34 +++ .../src/control_center_main.cpp | 122 +++++++++ .../src/control_center_node.cpp | 256 ++++++++++++++++++ .../src/include/control_center_node.hpp | 117 ++++++++ .../test/config/control_center.param.yaml | 4 + .../test/test_cc_heartbeat.cpp | 127 +++++++++ .../test/test_cc_init_shutdown.cpp | 59 ++++ .../test/test_cc_registering.cpp | 133 +++++++++ .../test/test_constants.hpp | 26 ++ .../test/test_utility.hpp | 174 ++++++++++++ .../CMakeLists.txt | 29 ++ common/autoware_control_center_msgs/README.md | 3 + .../msg/Heartbeat.msg | 14 + .../msg/NodeReport.msg | 11 + .../msg/NodeReports.msg | 4 + .../msg/NodeStatusActivity.msg | 10 + .../msg/NodeStatusOperational.msg | 9 + .../msg/ResultDeregistration.msg | 8 + .../msg/ResultRegistration.msg | 7 + .../msg/ResultService.msg | 6 + .../autoware_control_center_msgs/package.xml | 26 ++ .../srv/Deregister.srv | 4 + .../srv/Register.srv | 5 + common/autoware_node/CMakeLists.txt | 16 ++ common/autoware_node/COLCON_IGNORE | 0 common/autoware_node/README.md | 37 +++ .../include/autoware/node/node.hpp | 140 ++++++++++ .../autoware/node/visibility_control.hpp | 26 ++ common/autoware_node/package.xml | 24 ++ common/autoware_node/src/node.cpp | 161 +++++++++++ common/autoware_node/test/test_node.cpp | 35 +++ common/autoware_node/test/test_node.hpp | 18 ++ common/autoware_test_node/CMakeLists.txt | 17 ++ common/autoware_test_node/COLCON_IGNORE | 0 common/autoware_test_node/README.md | 0 .../launch/autoware_nodes.launch | 7 + common/autoware_test_node/package.xml | 27 ++ .../src/include/test_node.hpp | 38 +++ common/autoware_test_node/src/test_node.cpp | 42 +++ common/autoware_test_node/src/test_pub.cpp | 64 +++++ .../test/test_test_node.cpp | 15 + .../test/test_test_node.hpp | 18 ++ 46 files changed, 2045 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 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/src/include/control_center_node.hpp 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/msg/ResultService.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/COLCON_IGNORE create mode 100644 common/autoware_node/README.md 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_node.cpp create mode 100644 common/autoware_node/test/test_node.hpp create mode 100644 common/autoware_test_node/CMakeLists.txt create mode 100644 common/autoware_test_node/COLCON_IGNORE create mode 100644 common/autoware_test_node/README.md create mode 100644 common/autoware_test_node/launch/autoware_nodes.launch 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/src/test_pub.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..72f2f1d2 --- /dev/null +++ b/common/autoware_control_center/CMakeLists.txt @@ -0,0 +1,61 @@ +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 + src/include/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() + +# ament_add_ros_isolated_gtest(test_autoware_control_center ${TEST_SOURCES}) +# target_include_directories(test_autoware_control_center PRIVATE src/include) +# target_link_libraries(test_autoware_control_center ${PROJECT_NAME}) +# ament_target_dependencies(test_autoware_control_center +# rclcpp +# rclcpp_lifecycle +# autoware_utils +# autoware_control_center_msgs) +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..bfd29944 --- /dev/null +++ b/common/autoware_control_center/README.md @@ -0,0 +1,99 @@ +# Autoware Control Center + +## Abbreviations + +- **ACC:** Autoware Control Center +- **AN:** Autoware Node + +## 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::NodeReport` + +## Parameters + +| Name | Type | Default Value | Description | +| --------------------- | -------- | ------------- | ----------------------------------------------------------------------------------- | +| `lease_duration_ms` | `double` | `220.0` | If not received another heartbeat within this duration, AN will be considered dead. | +| `report_publish_rate` | `double` | `1.0` | Publish NodeReports rate (hz) | + +## 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 liveness. + +If the ACC does not receive a heartbeat from the Autoware node within the specified lease duration, +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/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..1112b581 --- /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_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..4ed1c565 --- /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 "include/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..1e4ad93b --- /dev/null +++ b/common/autoware_control_center/src/control_center_node.cpp @@ -0,0 +1,256 @@ +// 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/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_INFO(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_INFO(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_INFO(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() +{ + RCLCPP_INFO(this->get_logger(), "Publishing 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 = result_registration.result; + + if (result_registration.result != ResultRegistration::SUCCESS) { + response->result_service.result = ResultService::FAILURE; + RCLCPP_WARN(get_logger(), "Node registration failed for %s", node_name.c_str()); + return; + } + + response->uuid_node = node_uuid.value(); + response->result_service.result = ResultService::SUCCESS; +} + +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) { + response->result_service.result = ResultService::SUCCESS; + } else { + response->result_service.result = ResultService::FAILURE; + } +} + +} // namespace autoware::control_center diff --git a/common/autoware_control_center/src/include/control_center_node.hpp b/common/autoware_control_center/src/include/control_center_node.hpp new file mode 100644 index 00000000..fe0ca278 --- /dev/null +++ b/common/autoware_control_center/src/include/control_center_node.hpp @@ -0,0 +1,117 @@ +// 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 CONTROL_CENTER_NODE_HPP_ +#define 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 ResultService = autoware_control_center_msgs::msg::ResultService; + 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 // CONTROL_CENTER_NODE_HPP_ 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..aa0f46ef --- /dev/null +++ b/common/autoware_control_center/test/test_cc_heartbeat.cpp @@ -0,0 +1,127 @@ +// 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 / node_period_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(); + } + } + + rclcpp::NodeOptions node_options_; + std::shared_ptr executor_; + ControlCenter::SharedPtr control_center_; + std::thread thread_spin_; + double deadline_ms_ = 50.0; + double node_period_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_init = test::generate_hb_healthy(); + auto pub_hb = test::send_first_heartbeat(node, deadline_ms_, hb_healthy_init); + + double sleep_duration_ms = node_period_ms_ * 1.5; + ASSERT_LE(sleep_duration_ms, deadline_ms_); + + std::this_thread::sleep_for(std::chrono::duration(sleep_duration_ms)); + + autoware_control_center_msgs::msg::NodeReport report; + ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report)); + ASSERT_EQ(report.status_activity.status, hb_healthy_init->status_activity.status); + ASSERT_EQ(report.status_operational.status, hb_healthy_init->status_operational.status); + + test::send_heartbeat(node, pub_hb, test::generate_hb_healthy()); + std::this_thread::sleep_for(std::chrono::duration(deadline_ms_ * 0.8)); + test::send_heartbeat(node, pub_hb, test::generate_hb_healthy()); + + // Send another heartbeat to make sure it is not marked as dead + test::send_heartbeat(node, pub_hb, test::generate_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_ + node_period_ms_ * 1.5)); + + // Expect the node to be marked as dead + autoware_control_center_msgs::msg::NodeReport report; + ASSERT_TRUE(test::wait_for_node_report(node->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(node_period_ms_ * 2.0)); + ASSERT_TRUE(test::wait_for_node_report(node->get_fully_qualified_name(), report)); + ASSERT_TRUE(report.is_alive); + ASSERT_EQ(report.status_activity.status, hb_healthy->status_activity.status); + ASSERT_EQ(report.status_operational.status, hb_healthy->status_operational.status); +} + +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..152ad55e --- /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..f40ee667 --- /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..f286eff8 --- /dev/null +++ b/common/autoware_control_center/test/test_utility.hpp @@ -0,0 +1,174 @@ +// 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_service.result != autoware_control_center_msgs::msg::ResultService::SUCCESS || + 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_service.result == + autoware_control_center_msgs::msg::ResultService::SUCCESS && + 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) +{ + 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::seconds(5)) != + 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..6a8070eb --- /dev/null +++ b/common/autoware_control_center_msgs/CMakeLists.txt @@ -0,0 +1,29 @@ +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" + "msg/ResultService.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..984c9f56 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/Heartbeat.msg @@ -0,0 +1,14 @@ +# 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 + +# 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 0 + +# 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..5ee760e0 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeReport.msg @@ -0,0 +1,11 @@ +# Periodic report from the Autoware Control Center on the status of a registered node. + +# Node information +string name +unique_identifier_msgs/UUID uuid +autoware_control_center_msgs/NodeStatusActivity status_activity +autoware_control_center_msgs/NodeStatusOperational status_operational +builtin_interfaces/Time stamp_last_heartbeat +bool is_alive +builtin_interfaces/Time stamp_registration +uint16 count_registered 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..b9646c36 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/ResultDeregistration.msg @@ -0,0 +1,8 @@ +# 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 + +# Result of the deregistration +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..d078f621 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/ResultRegistration.msg @@ -0,0 +1,7 @@ +# 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 + +# Result of the registration +uint8 result diff --git a/common/autoware_control_center_msgs/msg/ResultService.msg b/common/autoware_control_center_msgs/msg/ResultService.msg new file mode 100644 index 00000000..393cebc6 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/ResultService.msg @@ -0,0 +1,6 @@ +# Enumeration of possible results for a service call. +uint8 FAILURE=0 +uint8 SUCCESS=1 + +# Result of the service call +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..2a105b3f --- /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..43024beb --- /dev/null +++ b/common/autoware_control_center_msgs/srv/Deregister.srv @@ -0,0 +1,4 @@ +unique_identifier_msgs/UUID uuid_node +--- +autoware_control_center_msgs/ResultService result_service +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..566ae9f4 --- /dev/null +++ b/common/autoware_control_center_msgs/srv/Register.srv @@ -0,0 +1,5 @@ +string node_name_with_namespace +--- +autoware_control_center_msgs/ResultService result_service +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..538dbdf5 --- /dev/null +++ b/common/autoware_node/CMakeLists.txt @@ -0,0 +1,16 @@ +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) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_autoware_node test/test_node.cpp) + target_link_libraries(test_autoware_node ${PROJECT_NAME}) +endif() + +ament_auto_package() diff --git a/common/autoware_node/COLCON_IGNORE b/common/autoware_node/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/common/autoware_node/README.md b/common/autoware_node/README.md new file mode 100644 index 00000000..dc386368 --- /dev/null +++ b/common/autoware_node/README.md @@ -0,0 +1,37 @@ +# Autoware Node + +## Overview + +Autoware Node 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 _Autoware_control_center_ (ACC), report node state, publish heartbeat and subscribe to monitored topics. 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 + +You can use _autoware_node_ as a base class for any node in Autoware.Core system. There is an example package _test_node_ which shows how _autoware_node_ communicate with ACC and other _autoware_nodes_. You can check it for more information. + +## Design + +_Autoware_node_ inherits from ROS 2 [_lifecycle_node_](https://design.ros2.org/articles/node_lifecycle.html) and has all basic functions of it. + +Below are the main add-ons and how they work. + +### Registration + +After startup each _autoware_node_ tries to register itself to ACC via a service call of _AutowareNodeRegister_. It happens in the dedicated timer. The timer will stop after the successful registration. + +### De-registration + +If _autoware_node_ receives a request to it's _ControlCenterDeregister_ service. It will disable the flag _registered_ and it will startup timer which control registration client. + +### Error state + +_Autoware_node_ has `send_state` method to send it's state to ACC via _AutowareNodeError_ service. You need to provide `AutowareNodeState` and log message as parameters to the method. + +### Heartbeat + +The heartbeat publisher is configured to publish the heartbeat message each 200 ms. You can change it by the `period` parameter during the launch of _autoware_node_. Be aware that you will also need to configure ACC accordingly. + +Heartbeat functionality is based on ros2 [software_watchdogs](https://github.com/ros-safety/software_watchdogs) package. + +### Monitored subscription + +_Autoware_node_ provides the `create_monitored_subscription` method. It wraps around a standard `create_subscription` method and adds a function to monitor a frequency of messages received in the topic. If it violates the condition provided in the `hz` parameter of the method _autoware_node_ will send an error state to the ACC. 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..061e806f --- /dev/null +++ b/common/autoware_node/include/autoware/node/node.hpp @@ -0,0 +1,140 @@ +// 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 + +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()); + +protected: + /*! + Create subscription to provided topic with topic_name and monitors period of received messages. + If desired period is violated the Autoware Node informs the Autoware Control Center. + \param[topic_name] The name of the monitored topic. + \param[hz] The desired message frequency of the topic. + \param[qos] The desired QoS for the topic. + */ + template < + typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> + std::shared_ptr create_monitored_subscription( + const std::string & topic_name, const float hz, const rclcpp::QoS & qos, CallbackT && callback, + const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strategy = + (MessageMemoryStrategyT::create_default())) + { + // create proper qos based on input parameter + // update lease duration and deadline in qos + RCLCPP_DEBUG(get_logger(), "Create monitored subscription to topic %s", topic_name.c_str()); + std::chrono::milliseconds lease_duration{ + static_cast(1.0 / hz * 1000 * 1.1)}; // add 10 % gap to lease duration (buffer) + rclcpp::QoS qos_profile = qos; + qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + .liveliness_lease_duration(lease_duration) + .deadline(lease_duration); + + rclcpp::SubscriptionOptions sub_options = options; + sub_options.event_callbacks.deadline_callback = + [=](rclcpp::QOSDeadlineRequestedInfo & event) -> void { + RCLCPP_ERROR( + get_logger(), "Requested deadline missed - total %d delta %d", event.total_count, + event.total_count_change); + // NodeError service call + std::string msg = "Deadline for topic " + topic_name + " was missed."; + autoware_control_center_msgs::msg::NodeState node_state; + node_state.status = autoware_control_center_msgs::msg::NodeState::ERROR; + send_state(node_state, msg); + }; + + sub_options.event_callbacks.liveliness_callback = + [=](rclcpp::QOSLivelinessChangedInfo & event) -> void { + RCLCPP_DEBUG(get_logger(), "%s topic liveliness info changed", topic_name.c_str()); + RCLCPP_DEBUG(get_logger(), " alive_count: %d", event.alive_count); + RCLCPP_DEBUG(get_logger(), " not_alive_count: %d", event.not_alive_count); + RCLCPP_DEBUG(get_logger(), " alive_count_change: %d", event.alive_count_change); + RCLCPP_DEBUG(get_logger(), " not_alive_count_change: %d", event.not_alive_count_change); + if (event.alive_count == 0) { + RCLCPP_ERROR(get_logger(), "%s topic publisher is not alive.", topic_name.c_str()); + // NodeError service call + std::string msg = topic_name + " topic publisher is not alive."; + autoware_control_center_msgs::msg::NodeState node_state; + node_state.status = autoware_control_center_msgs::msg::NodeState::ERROR; + send_state(node_state, msg); + } + }; + + return create_subscription( + topic_name, qos_profile, std::forward(callback), sub_options, msg_mem_strategy); + } + + rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_; + + rclcpp::Client::SharedPtr cli_register_; + rclcpp::Client::SharedPtr cli_report_status_; + + rclcpp::Publisher::SharedPtr pub_heartbeat_; + + rclcpp::TimerBase::SharedPtr timer_heartbeat_; + rclcpp::TimerBase::SharedPtr timer_registration_; + + const std::string self_name_; + bool is_registered_; + unique_identifier_msgs::msg::UUID self_uuid; + unique_identifier_msgs::msg::UUID acc_uuid; + +private: + using FutureRegister = rclcpp::Client::SharedFuture; + using FutureReportStatus = + rclcpp::Client::SharedFuture; + + uint16_t sequence_number_; + + void on_tick_registration(); + void on_tick_heartbeat(); + void on_register(FutureRegister future); + void on_report_status(FutureReportStatus future); + + void send_state( + const autoware_control_center_msgs::msg::NodeState & node_state, std::string message); +}; + +} // 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..596e453e --- /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..f0eb24ca --- /dev/null +++ b/common/autoware_node/src/node.cpp @@ -0,0 +1,161 @@ +// 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 "autoware_control_center_msgs/msg/node_status.hpp" +#include "autoware_control_center_msgs/srv/register.hpp" + +#include + +constexpr std::chrono::milliseconds lease_delta( + 20); ///< Buffer added to heartbeat to define lease. + +namespace autoware::node +{ + +Node::Node( + const std::string & node_name, const std::string & ns, const rclcpp::NodeOptions & options) +: LifecycleNode(node_name, ns, options), + self_name_{ns + "/" + node_name}, + is_registered_{false}, + sequence_number_{0} +{ + RCLCPP_DEBUG(get_logger(), "Node::Node()"); + declare_parameter("heartbeat_period", 200); + declare_parameter("register_timer_period", 500); + std::chrono::milliseconds heartbeat_period(get_parameter("heartbeat_period").as_int()); + std::chrono::milliseconds register_timer_period(get_parameter("register_timer_period").as_int()); + + // The granted lease is essentially infinite here, i.e., only reader/watchdog will notify + // violations. XXX causes segfault for cyclone dds, hence pass explicit lease life > heartbeat. + rclcpp::QoS qos_profile(1); + qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + .liveliness_lease_duration(heartbeat_period + lease_delta) + .deadline(heartbeat_period + lease_delta); + + pub_heartbeat_ = this->create_publisher( + "~/heartbeat", qos_profile); + timer_heartbeat_ = + this->create_wall_timer(heartbeat_period, std::bind(&Node::on_tick_heartbeat, this)); + + callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + cli_register_ = create_client( + "/autoware/control_center/srv/register", rmw_qos_profile_default, callback_group_mut_ex_); + + cli_report_status_ = create_client( + "/autoware/control_center/srv/report_status", rmw_qos_profile_default, callback_group_mut_ex_); + + timer_registration_ = + this->create_wall_timer(register_timer_period, std::bind(&Node::on_tick_registration, this)); +} + +void Node::on_tick_registration() +{ + RCLCPP_DEBUG(get_logger(), "Register callback"); + if (is_registered_) { + RCLCPP_DEBUG(get_logger(), "It was registered before"); + 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->name_node = self_name_; + + cli_register_->async_send_request( + req, std::bind(&Node::on_register, this, std::placeholders::_1)); + RCLCPP_DEBUG(get_logger(), "Sent request"); + + const std::string msg = self_name_ + " node started"; + autoware_control_center_msgs::msg::NodeState node_state; + node_state.status = autoware_control_center_msgs::msg::NodeState::NORMAL; + send_state(node_state, msg); + RCLCPP_DEBUG(get_logger(), "Sent node state"); +} + +void Node::on_tick_heartbeat() +{ + auto message = autoware_control_center_msgs::msg::Heartbeat(); + message.stamp = this->get_clock()->now(); + message.sequence_number = sequence_number_++; + RCLCPP_DEBUG(this->get_logger(), "Publishing heartbeat, sent at [%i]", message.stamp.sec); + pub_heartbeat_->publish(message); +} + +void Node::send_state( + const autoware_control_center_msgs::msg::NodeState & node_state, std::string message) +{ + if (!cli_report_status_->service_is_ready()) { + RCLCPP_WARN(get_logger(), "%s is unavailable", cli_report_status_->get_service_name()); + return; + } + autoware_control_center_msgs::srv::ReportState::Request::SharedPtr req = + std::make_shared(); + + req->name_node = self_name_; + req->state = node_state; + req->message = std::move(message); + + cli_report_status_->async_send_request( + req, std::bind(&Node::on_report_status, this, std::placeholders::_1)); + RCLCPP_DEBUG(get_logger(), "Send node state"); +} + +// performance-unnecessary-value-param +// TODO(xmfcx): add the line above the line below once next cpplint is released (1.7.0 or 2.0.0) +// NOLINTNEXTLINE +void Node::on_register(FutureRegister future) +{ + const auto & response = future.get(); + std::string str_uuid = autoware_utils::to_hex_string(response->uuid_node); + RCLCPP_DEBUG(get_logger(), "response: %d, %s", response->status.status, str_uuid.c_str()); + + if (response->status.status == autoware_control_center_msgs::msg::NodeStatus::SUCCESS) { + is_registered_ = true; + self_uuid = response->uuid_node; + RCLCPP_DEBUG(get_logger(), "Node was registered"); + timer_registration_->cancel(); + RCLCPP_DEBUG(get_logger(), "Register timer was cancelled"); + } else { + RCLCPP_ERROR(get_logger(), "Failed to register node"); + } +} + +// performance-unnecessary-value-param +// TODO(xmfcx): add the line above the line below once next cpplint is released (1.7.0 or 2.0.0) +// NOLINTNEXTLINE +void Node::on_report_status(FutureReportStatus future) +{ + const auto & response = future.get(); + std::string str_uuid = autoware_utils::to_hex_string(response->uuid_node); + RCLCPP_DEBUG( + get_logger(), "response: %d, %s, %s", response->status.status, str_uuid.c_str(), + response->log_response.c_str()); + + if (response->status.status == autoware_control_center_msgs::msg::NodeStatus::SUCCESS) { + RCLCPP_DEBUG(get_logger(), "Node state was received by ACC"); + } else { + RCLCPP_ERROR(get_logger(), "Failed to send Node state to ACC"); + } +} + +} // namespace autoware::node diff --git a/common/autoware_node/test/test_node.cpp b/common/autoware_node/test/test_node.cpp new file mode 100644 index 00000000..e27c57f2 --- /dev/null +++ b/common/autoware_node/test/test_node.cpp @@ -0,0 +1,35 @@ +// 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_node.hpp" + +#include "autoware/node/node.hpp" +#include "gtest/gtest.h" +// #include "rclcpp_components/rclcpp.hpp" + +// class AutowareNodeTest : public ::testing::Test +// { +// public: +// void SetUp() override +// { +// rclcpp::init(0, nullptr); +// autoware_node_ = +// std::make_shared + + + + + + diff --git a/common/autoware_test_node/package.xml b/common/autoware_test_node/package.xml new file mode 100644 index 00000000..158b368e --- /dev/null +++ b/common/autoware_test_node/package.xml @@ -0,0 +1,27 @@ + + + + autoware_test_node + 0.0.0 + Test package for Autoware Control Center and 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..9960bdcc --- /dev/null +++ b/common/autoware_test_node/src/include/test_node.hpp @@ -0,0 +1,38 @@ +// 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 + +#include + +namespace autoware::test_node +{ + +class TestNode : public autoware::node::Node +{ +public: + explicit TestNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void topic_callback(const std_msgs::msg::String::SharedPtr msg); + rclcpp::Subscription::SharedPtr monitored_subscription_; +}; + +} // 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..0e36f361 --- /dev/null +++ b/common/autoware_test_node/src/test_node.cpp @@ -0,0 +1,42 @@ +// 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", autoware::node::Node::self_name_.c_str()); + + using std::placeholders::_1; + monitored_subscription_ = + autoware::node::Node::create_monitored_subscription( + "topic", 10, 10, std::bind(&TestNode::topic_callback, this, _1)); +} + +void TestNode::topic_callback(const std_msgs::msg::String::SharedPtr msg) +{ + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); +} + +} // 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/src/test_pub.cpp b/common/autoware_test_node/src/test_pub.cpp new file mode 100644 index 00000000..95cc73e1 --- /dev/null +++ b/common/autoware_test_node/src/test_pub.cpp @@ -0,0 +1,64 @@ +// 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 +#include + +/* This example creates a subclass of Node and uses std::bind() to register a + * member function as a callback from the timer. */ + +class MinimalPublisher : public rclcpp::Node +{ +public: + MinimalPublisher() : Node("minimal_publisher") + { + rclcpp::QoS qos_profile(10); + declare_parameter("hz", 10.0); + const double hz = get_parameter("hz").as_double(); + std::chrono::milliseconds timer_period{static_cast(1.0 / hz * 1000)}; + std::chrono::milliseconds lease_duration{static_cast(1.0 / hz * 1000 * 1.1)}; + qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + .liveliness_lease_duration(lease_duration) + .deadline(lease_duration); + publisher_ = this->create_publisher("topic", qos_profile); + timer_ = + this->create_wall_timer(timer_period, std::bind(&MinimalPublisher::on_tick_timer, this)); + } + +private: + void on_tick_timer() + { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + publisher_->publish(message); + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_ = 0; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} 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_