diff --git a/common/autoware_control_center/CMakeLists.txt b/common/autoware_control_center/CMakeLists.txt new file mode 100644 index 00000000..6a5f4785 --- /dev/null +++ b/common/autoware_control_center/CMakeLists.txt @@ -0,0 +1,37 @@ +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/node_registry.cpp + src/include/control_center_node.hpp + src/include/node_registry.hpp +) + +#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) + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + 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..bd59db7d --- /dev/null +++ b/common/autoware_control_center/README.md @@ -0,0 +1,104 @@ +# 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` + +#### ReportState + +Reports the state of an Autoware node to the ACC. + +- **Topic:** `/autoware/control_center/srv/report_state` +- **Type:** `autoware_control_center_msgs::srv::ReportState` + +### 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. + +- **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..bbfd1f69 --- /dev/null +++ b/common/autoware_control_center/config/control_center.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + lease_duration_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..99ad6219 --- /dev/null +++ b/common/autoware_control_center/package.xml @@ -0,0 +1,32 @@ + + + + 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 + + + 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..65b99dce --- /dev/null +++ b/common/autoware_control_center/src/control_center_main.cpp @@ -0,0 +1,93 @@ +// 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 + +const char * LOCK_FILE = "/tmp/autoware_control_center_node.lock"; + +bool lock_file() +{ + int fd = open(LOCK_FILE, O_CREAT | O_RDWR, 0666); + if (fd == -1) { + RCLCPP_FATAL(rclcpp::get_logger(""), "Failed to open lock file"); + return false; + } + + if (flock(fd, LOCK_EX | LOCK_NB) == -1) { + if (errno == EWOULDBLOCK) { + RCLCPP_FATAL(rclcpp::get_logger(""), "Another instance is already running"); + } else { + RCLCPP_FATAL(rclcpp::get_logger(""), "Failed to lock file"); + } + close(fd); + return false; + } + + // Keep the file descriptor open to maintain the lock + return true; +} + +void unlock_file(int fd) +{ + if (fd != -1) { + flock(fd, LOCK_UN); + close(fd); + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + // Lock file to prevent multiple instances on the same machine + int lock_fd = open(LOCK_FILE, O_CREAT | O_RDWR, 0666); + if (!lock_file()) { + return 1; + } + + // Check if node already exists to prevent multiple instances across the network + // It's a bit slow but better than nothing + const auto node_name_with_namespace = std::string("/autoware/control_center"); + + auto 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(); + }; + + if (node_already_exists(node_name_with_namespace)) { + RCLCPP_FATAL( + rclcpp::get_logger(""), "Node %s already exists", node_name_with_namespace.c_str()); + unlock_file(lock_fd); + throw std::runtime_error("Node already exists"); + } + + // 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(); + 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..923268d8 --- /dev/null +++ b/common/autoware_control_center/src/control_center_node.cpp @@ -0,0 +1,186 @@ +// 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/node_registry.hpp" + +#include + +#include +#include + +#include + +namespace autoware::control_center +{ +ControlCenter::ControlCenter(const rclcpp::NodeOptions & options) +: LifecycleNode("control_center", "autoware", options), + lease_duration_ms_{declare_parameter("lease_duration_ms")}, + report_publish_rate_{declare_parameter("report_publish_rate")} +{ + RCLCPP_DEBUG(get_logger(), "ControlCenter is initialized"); + + timer_publish_reports_ = create_wall_timer( + std::chrono::duration(1.0 / report_publish_rate_), + std::bind(&ControlCenter::on_tick_publish_reports, this)); + + callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + using std::placeholders::_1; + using std::placeholders::_2; + srv_register_ = create_service( + "~/srv/register", std::bind(&ControlCenter::on_register_node, this, _1, _2), + rmw_qos_profile_services_default, callback_group_mut_ex_); + srv_deregister_ = create_service( + "~/srv/deregister", std::bind(&ControlCenter::on_deregister_node, this, _1, _2), + rmw_qos_profile_services_default, callback_group_mut_ex_); + srv_report_state_ = create_service( + "~/srv/report_state", std::bind(&ControlCenter::on_report_state, this, _1, _2), + rmw_qos_profile_services_default, callback_group_mut_ex_); + + pub_reports_ = create_publisher("~/reports", 1); + + acc_uuid_ = autoware_utils::generate_uuid(); +} + +void ControlCenter::on_register_node( + const Register::Request::SharedPtr request, const Register::Response::SharedPtr response) +{ + RCLCPP_DEBUG(get_logger(), "on_register_node is called from %s", request->name_node.c_str()); + + std::optional node_uuid = + node_registry_.register_node(request->name_node, autoware_utils::generate_uuid()); + + if (node_uuid == std::nullopt) { + response->uuid_node = autoware_utils::generate_default_uuid(); + response->status.status = Register::Response::_status_type::FAILURE; + } else { + NodeState un_state; + un_state.status = NodeState::UNKNOWN; + // alive, last_heartbeat, node_report, state + node_status_map_.insert({request->name_node, {false, this->now(), "", un_state}}); + // Create heartbeat sub + rclcpp::Subscription::SharedPtr + heartbeat_node_sub = create_heartbeat_sub(request->name_node); + heartbeat_sub_map_.insert({request->name_node, heartbeat_node_sub}); + RCLCPP_DEBUG(get_logger(), "Subscribed to topic %s", heartbeat_node_sub->get_topic_name()); + response->uuid_node = node_uuid.value(); + response->status.status = Register::Response::_status_type::SUCCESS; + } +} + +void ControlCenter::on_deregister_node( + const Deregister::Request::SharedPtr request, const Deregister::Response::SharedPtr response) +{ + RCLCPP_DEBUG(get_logger(), "on_deregister_node is called from %s", request->name_node.c_str()); + + std::optional node_uuid = + node_registry_.deregister_node(request->name_node); + + if (node_uuid == std::nullopt) { + response->uuid_node = autoware_utils::generate_default_uuid(); + response->status.status = Deregister::Response::_status_type::FAILURE; + } else { + response->uuid_node = node_uuid.value(); + response->status.status = Deregister::Response::_status_type::SUCCESS; + } +} + +rclcpp::Subscription::SharedPtr ControlCenter::create_heartbeat_sub( + const std::string & node_name) +{ + RCLCPP_DEBUG(get_logger(), "Create heart sub is called."); + auto qos_profile = rclcpp::QoS(10); + qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + .liveliness_lease_duration(lease_duration_ms_); + + rclcpp::SubscriptionOptions heartbeat_sub_options; + std::function bound_liveliness_callback_func = + std::bind(&ControlCenter::liveliness_callback, this, std::placeholders::_1, node_name); + heartbeat_sub_options.event_callbacks.liveliness_callback = bound_liveliness_callback_func; + + const std::string topic_name = node_name + "/heartbeat"; + RCLCPP_DEBUG(get_logger(), "Topic to subscribe is %s", topic_name.c_str()); + rclcpp::Subscription::SharedPtr heartbeat_sub; + std::function bound_heartbeat_callback_func = + std::bind(&ControlCenter::heartbeat_callback, this, std::placeholders::_1, node_name); + heartbeat_sub = create_subscription( + topic_name, qos_profile, bound_heartbeat_callback_func, heartbeat_sub_options); + + return heartbeat_sub; +} + +void ControlCenter::on_tick_publish_reports() +{ + NodeReports msg; + const rclcpp::Time stamp = this->now(); + msg.stamp = stamp; + for (auto const & [name, info] : node_status_map_) { + autoware_control_center_msgs::msg::NodeReport report; + report.uuid_node = node_registry_.get_uuid(name).value(); + report.name_node = name; + report.alive = info.alive; + report.last_node_state = info.state; + report.node_report = info.node_report; + report.last_heartbeat = stamp - info.last_heartbeat; + msg.nodes.push_back(report); + } + pub_reports_->publish(msg); +} + +void ControlCenter::on_report_state( + const ReportState::Request::SharedPtr request, const ReportState::Response::SharedPtr response) +{ + RCLCPP_DEBUG(get_logger(), "on_report_state is called from %s", request->name_node.c_str()); + // node_report + if (node_registry_.is_registered(request->name_node)) { + node_status_map_[request->name_node].node_report = request->message; + node_status_map_[request->name_node].state = request->state; + + response->status.status = ReportState::Response::_status_type::SUCCESS; + response->uuid_node = node_registry_.get_uuid(request->name_node).value(); + } else { + response->status.status = ReportState::Response::_status_type::FAILURE; + response->uuid_node = autoware_utils::generate_default_uuid(); + response->log_response = request->name_node + " node was not registered."; + } +} + +void ControlCenter::liveliness_callback( + rclcpp::QOSLivelinessChangedInfo & event, const std::string & node_name) +{ + RCLCPP_DEBUG(get_logger(), "Reader Liveliness changed event:"); + 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(), "Heartbeat was not received"); + node_status_map_[node_name].alive = false; + } +} + +void ControlCenter::heartbeat_callback( + const Heartbeat::SharedPtr msg, const std::string & node_name) +{ + RCLCPP_DEBUG(get_logger(), "Watchdog raised, heartbeat sent at [%d.x]", msg->stamp.sec); + node_status_map_[node_name].alive = true; + node_status_map_[node_name].last_heartbeat = msg->stamp; +} + +} // namespace autoware::control_center + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::control_center::ControlCenter) 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..e002ce7c --- /dev/null +++ b/common/autoware_control_center/src/include/control_center_node.hpp @@ -0,0 +1,100 @@ +// 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 "node_registry.hpp" + +#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; + explicit ControlCenter(const rclcpp::NodeOptions & options); + +private: + using Register = autoware_control_center_msgs::srv::Register; + using Deregister = autoware_control_center_msgs::srv::Deregister; + using ReportState = autoware_control_center_msgs::srv::ReportState; + using NodeReports = autoware_control_center_msgs::msg::NodeReports; + using Heartbeat = autoware_control_center_msgs::msg::Heartbeat; + using NodeState = autoware_control_center_msgs::msg::NodeState; + + enum class HealthState { Unknown = 0, Healthy = 1, Warning = 2, Error = 3 }; + + struct AutowareNodeStatus + { + bool alive; + rclcpp::Time last_heartbeat; + std::string node_report; + NodeState state; + }; + + const std::chrono::duration lease_duration_ms_; + const double report_publish_rate_; + + rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_; + + rclcpp::Service::SharedPtr srv_register_; + rclcpp::Service::SharedPtr srv_deregister_; + rclcpp::Service::SharedPtr srv_report_state_; + + rclcpp::Publisher::SharedPtr pub_reports_; + + rclcpp::TimerBase::SharedPtr timer_publish_reports_; + + NodeRegistry node_registry_; + std::unordered_map::SharedPtr> heartbeat_sub_map_; + std::unordered_map node_status_map_; + unique_identifier_msgs::msg::UUID acc_uuid_; + + void on_register_node( + Register::Request::SharedPtr request, Register::Response::SharedPtr response); + + void on_deregister_node( + Deregister::Request::SharedPtr request, Deregister::Response::SharedPtr response); + + void on_report_state( + ReportState::Request::SharedPtr request, ReportState::Response::SharedPtr response); + + void on_tick_publish_reports(); + + void liveliness_callback(rclcpp::QOSLivelinessChangedInfo & event, const std::string & node_name); + + void heartbeat_callback(Heartbeat::SharedPtr msg, const std::string & node_name); + + rclcpp::Subscription::SharedPtr create_heartbeat_sub(const std::string & node_name); +}; + +} // namespace autoware::control_center + +#endif // CONTROL_CENTER_NODE_HPP_ diff --git a/common/autoware_control_center/src/include/node_registry.hpp b/common/autoware_control_center/src/include/node_registry.hpp new file mode 100644 index 00000000..09b0f4b3 --- /dev/null +++ b/common/autoware_control_center/src/include/node_registry.hpp @@ -0,0 +1,57 @@ +// 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 NODE_REGISTRY_HPP_ +#define NODE_REGISTRY_HPP_ + +#include + +#include + +#include +#include +#include + +namespace autoware::control_center +{ +struct AutowareNodeInfo +{ + rclcpp::Time time_registering; + std::string name; + unique_identifier_msgs::msg::UUID uuid; + int num_registered; +}; + +class NodeRegistry +{ +public: + NodeRegistry() = default; + + std::optional register_node( + const std::string & name, const unique_identifier_msgs::msg::UUID & uuid); + + std::optional deregister_node(const std::string & name); + + bool is_registered(const std::string & name) const; + + std::optional get_uuid(const std::string & name) const; + + bool is_empty(); + +private: + std::unordered_map autoware_node_info_map_; +}; +} // namespace autoware::control_center + +#endif // NODE_REGISTRY_HPP_ diff --git a/common/autoware_control_center/src/node_registry.cpp b/common/autoware_control_center/src/node_registry.cpp new file mode 100644 index 00000000..9fe1a3df --- /dev/null +++ b/common/autoware_control_center/src/node_registry.cpp @@ -0,0 +1,73 @@ +// 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/node_registry.hpp" + +#include + +namespace autoware::control_center +{ +std::optional NodeRegistry::register_node( + const std::string & name, const unique_identifier_msgs::msg::UUID & uuid) +{ + if (is_registered(name)) { + RCLCPP_DEBUG( + rclcpp::get_logger("NodeRegistry"), "Node %s has been registered before. Register again.", + name.c_str()); + autoware_node_info_map_[name].time_registering = rclcpp::Clock().now(); + autoware_node_info_map_[name].uuid = uuid; + autoware_node_info_map_[name].num_registered += 1; + } else { + autoware_node_info_map_[name] = {rclcpp::Clock().now(), name, uuid, 1}; + RCLCPP_DEBUG(rclcpp::get_logger("NodeRegistry"), "Node %s is registered.", name.c_str()); + } + + return autoware_node_info_map_.at(name).uuid; +} + +std::optional NodeRegistry::deregister_node( + const std::string & name) +{ + if (!is_registered(name)) { + RCLCPP_WARN( + rclcpp::get_logger("NodeRegistry"), "Node %s is not registered. Ignoring.", name.c_str()); + return std::nullopt; + } + unique_identifier_msgs::msg::UUID node_uuid = autoware_node_info_map_.at(name).uuid; + autoware_node_info_map_.erase(name); + + RCLCPP_DEBUG(rclcpp::get_logger("NodeRegistry"), "Node %s is deregistered.", name.c_str()); + return node_uuid; +} + +bool NodeRegistry::is_registered(const std::string & name) const +{ + return autoware_node_info_map_.find(name) != autoware_node_info_map_.end(); +} + +std::optional NodeRegistry::get_uuid( + const std::string & name) const +{ + if (!is_registered(name)) { + return std::nullopt; + } + + return autoware_node_info_map_.at(name).uuid; +} + +bool NodeRegistry::is_empty() +{ + return autoware_node_info_map_.empty(); +} +} // namespace autoware::control_center diff --git a/common/autoware_control_center/test/test_control_center.cpp b/common/autoware_control_center/test/test_control_center.cpp new file mode 100644 index 00000000..dc87fa2d --- /dev/null +++ b/common/autoware_control_center/test/test_control_center.cpp @@ -0,0 +1,180 @@ +// 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 +#include +#include + +class ControlCenterTest : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("lease_duration_ms", 220.0); + node_options.append_parameter_override("report_publish_rate", 1.0); + control_center_ = std::make_shared(node_options); + } + + void TearDown() override { rclcpp::shutdown(); } + autoware::control_center::ControlCenter::SharedPtr control_center_; +}; + +TEST_F(ControlCenterTest, RegisterNode) +{ + auto client = control_center_->create_client( + "/autoware/control_center/srv/register"); + if (!client->wait_for_service(std::chrono::seconds(20))) { + ASSERT_TRUE(false) << "Node register service not available after waiting"; + } + + auto request = std::make_shared(); + request->name_node = "test_node"; + + auto result = client->async_send_request(request); + auto ret = rclcpp::spin_until_future_complete( + control_center_, result, std::chrono::seconds(5)); // Wait for the result. + + ASSERT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + + EXPECT_EQ(1, result.get()->status.status); +} + +TEST_F(ControlCenterTest, DeregisterNode) +{ + std::string node_name = "test_node"; + auto client_reg = control_center_->create_client( + "/autoware/control_center/srv/register"); + + if (!client_reg->wait_for_service(std::chrono::seconds(20))) { + ASSERT_TRUE(false) << "Node register service not available after waiting"; + } + // Register node first + auto request_reg = std::make_shared(); + request_reg->name_node = node_name; + + auto result_reg = client_reg->async_send_request(request_reg); + auto ret = rclcpp::spin_until_future_complete( + control_center_, result_reg, std::chrono::seconds(5)); // Wait for the result. + + ASSERT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + auto result_reg_payload = result_reg.get(); + EXPECT_EQ(1, result_reg_payload->status.status) << "Node register request fail"; + unique_identifier_msgs::msg::UUID uuid_node = result_reg_payload->uuid_node; + + // cspell:ignore dereg + // Deregister node + auto client_dereg = control_center_->create_client( + "/autoware/control_center/srv/deregister"); + if (!client_dereg->wait_for_service(std::chrono::seconds(20))) { + ASSERT_TRUE(false) << "Node deregister service not available after waiting"; + } + + auto request_dereg = std::make_shared(); + request_dereg->name_node = node_name; + + auto result_dereg = client_dereg->async_send_request(request_dereg); + ret = rclcpp::spin_until_future_complete(control_center_, result_dereg, std::chrono::seconds(5)); + + ASSERT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + auto result_dereg_payload = result_dereg.get(); + EXPECT_EQ(1, result_dereg_payload->status.status) << "Node deregister request fail"; + EXPECT_EQ(uuid_node, result_dereg_payload->uuid_node) << "ACC return wrong uuid"; +} + +TEST_F(ControlCenterTest, NodeErrorServiceNotRegistered) +{ + std::string node_name = "test_node"; + auto client = control_center_->create_client( + "/autoware/control_center/srv/report_state"); + + if (!client->wait_for_service(std::chrono::seconds(20))) { + ASSERT_TRUE(false) << "Node error service not available after waiting"; + } + + auto error_request = std::make_shared(); + error_request->name_node = node_name; + autoware_control_center_msgs::msg::NodeState node_state; + node_state.status = autoware_control_center_msgs::msg::NodeState::NORMAL; + error_request->state = node_state; + error_request->message = "test message"; + + auto result_error = client->async_send_request(error_request); + auto ret = + rclcpp::spin_until_future_complete(control_center_, result_error, std::chrono::seconds(5)); + + ASSERT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + auto result_error_payload = result_error.get(); + EXPECT_EQ(0, result_error_payload->status.status) << "Node error request wrong status"; + std::string log_msg = node_name + " node was not registered."; + EXPECT_EQ(log_msg, result_error_payload->log_response) << "Received wrong log response"; + EXPECT_EQ(autoware_utils::generate_default_uuid(), result_error_payload->uuid_node) + << "Received wrong uuid"; +} + +TEST_F(ControlCenterTest, NodeErrorService) +{ + std::string node_name = "test_node"; + auto client_reg = control_center_->create_client( + "/autoware/control_center/srv/register"); + + if (!client_reg->wait_for_service(std::chrono::seconds(20))) { + ASSERT_TRUE(false) << "Node register service not available after waiting"; + } + // Register node first + auto request_reg = std::make_shared(); + request_reg->name_node = node_name; + + auto result_reg = client_reg->async_send_request(request_reg); + auto ret_reg = rclcpp::spin_until_future_complete( + control_center_, result_reg, std::chrono::seconds(5)); // Wait for the result. + + ASSERT_EQ(ret_reg, rclcpp::FutureReturnCode::SUCCESS); + auto result_reg_payload = result_reg.get(); + EXPECT_EQ(1, result_reg_payload->status.status) << "Node register request fail"; + unique_identifier_msgs::msg::UUID uuid_node = result_reg_payload->uuid_node; + + auto error_client = + control_center_->create_client( + "/autoware/control_center/srv/report_state"); + + if (!error_client->wait_for_service(std::chrono::seconds(20))) { + ASSERT_TRUE(false) << "Node error service not available after waiting"; + } + + auto error_request = std::make_shared(); + error_request->name_node = node_name; + autoware_control_center_msgs::msg::NodeState node_state; + node_state.status = autoware_control_center_msgs::msg::NodeState::NORMAL; + error_request->state = node_state; + error_request->message = "test message"; + + auto result_error = error_client->async_send_request(error_request); + auto ret_err = + rclcpp::spin_until_future_complete(control_center_, result_error, std::chrono::seconds(5)); + + ASSERT_EQ(ret_err, rclcpp::FutureReturnCode::SUCCESS); + auto result_error_payload = result_error.get(); + EXPECT_EQ(1, result_error_payload->status.status) << "Node error request wrong status"; + EXPECT_EQ(uuid_node, result_error_payload->uuid_node) << "Received wrong uuid"; +} diff --git a/common/autoware_control_center/test/test_node_registry.cpp b/common/autoware_control_center/test/test_node_registry.cpp new file mode 100644 index 00000000..9f2eb422 --- /dev/null +++ b/common/autoware_control_center/test/test_node_registry.cpp @@ -0,0 +1,92 @@ +// 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 + +// Test fixture for NodeRegistry +class NodeRegistryTest : public ::testing::Test +{ +protected: + autoware::control_center::NodeRegistry registry; +}; + +TEST_F(NodeRegistryTest, RegisterNodeTest) +{ + std::string node_name = "test_node"; + unique_identifier_msgs::msg::UUID uuid; + uuid = autoware_utils::generate_uuid(); + + auto result = registry.register_node(node_name, uuid); + + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result.value(), uuid); + EXPECT_TRUE(registry.is_registered(node_name)); +} + +TEST_F(NodeRegistryTest, SameNameRegister) +{ + std::string node_name = "test_node"; + unique_identifier_msgs::msg::UUID first_uuid; + first_uuid = autoware_utils::generate_uuid(); + + auto first_result = registry.register_node(node_name, first_uuid); + ASSERT_TRUE(first_result.has_value()); + + unique_identifier_msgs::msg::UUID second_uuid; + second_uuid = autoware_utils::generate_uuid(); + auto second_result = registry.register_node(node_name, second_uuid); + + ASSERT_TRUE(second_result.has_value()); + EXPECT_EQ(second_result.value(), second_uuid); + EXPECT_TRUE(registry.is_registered(node_name)); +} + +TEST_F(NodeRegistryTest, DeregisterNodeTest) +{ + std::string node_name = "test_node"; + unique_identifier_msgs::msg::UUID uuid; + uuid = autoware_utils::generate_uuid(); + + registry.register_node(node_name, uuid); + auto result = registry.deregister_node(node_name); + + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result.value(), uuid); + EXPECT_FALSE(registry.is_registered(node_name)); +} + +TEST_F(NodeRegistryTest, GetUUIDTest) +{ + std::string node_name = "test_node"; + unique_identifier_msgs::msg::UUID uuid; + uuid = autoware_utils::generate_uuid(); + + registry.register_node(node_name, uuid); + auto result = registry.get_uuid(node_name); + + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result.value(), uuid); +} + +TEST_F(NodeRegistryTest, IsEmptyTest) +{ + EXPECT_TRUE(registry.is_empty()); + + registry.register_node("test_node", unique_identifier_msgs::msg::UUID()); + + EXPECT_FALSE(registry.is_empty()); +} diff --git a/common/autoware_control_center_msgs/CMakeLists.txt b/common/autoware_control_center_msgs/CMakeLists.txt new file mode 100644 index 00000000..45f85e16 --- /dev/null +++ b/common/autoware_control_center_msgs/CMakeLists.txt @@ -0,0 +1,27 @@ +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/NodeState.msg" + "msg/NodeStatus.msg" + "srv/Deregister.srv" + "srv/Register.srv" + "srv/ReportState.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..f2aef1f4 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/Heartbeat.msg @@ -0,0 +1,10 @@ +# Heartbeat message designed to monitor node liveliness with an additional +# sequence number to track message order and detect potential drops. + +# Two-integer timestamp that is expressed as seconds and nanoseconds. +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 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..38d5fd1f --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeReport.msg @@ -0,0 +1,8 @@ +# NodeStatus message designed to provide current status of Autoware node registered to Autoware Control Center + +unique_identifier_msgs/UUID uuid_node +string name_node +bool alive +autoware_control_center_msgs/NodeState last_node_state +string node_report +builtin_interfaces/Duration last_heartbeat 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..0cc15c50 --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeReports.msg @@ -0,0 +1,4 @@ +# NodeRegisterStatus message designed to provide the information about all nodes currently + +builtin_interfaces/Time stamp +NodeReport[] nodes diff --git a/common/autoware_control_center_msgs/msg/NodeState.msg b/common/autoware_control_center_msgs/msg/NodeState.msg new file mode 100644 index 00000000..78e1205a --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeState.msg @@ -0,0 +1,8 @@ +# Status States +uint8 UNKNOWN=0 +uint8 NORMAL=1 +uint8 WARNING=2 +uint8 ERROR=3 + +# Status +uint8 status diff --git a/common/autoware_control_center_msgs/msg/NodeStatus.msg b/common/autoware_control_center_msgs/msg/NodeStatus.msg new file mode 100644 index 00000000..239c68bc --- /dev/null +++ b/common/autoware_control_center_msgs/msg/NodeStatus.msg @@ -0,0 +1,6 @@ +# Status States +uint8 FAILURE=0 +uint8 SUCCESS=1 + +# Status +uint8 status 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..e674fdc9 --- /dev/null +++ b/common/autoware_control_center_msgs/srv/Deregister.srv @@ -0,0 +1,5 @@ +string name_node +--- +autoware_control_center_msgs/NodeStatus status +unique_identifier_msgs/UUID uuid_node +string log_response 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..e674fdc9 --- /dev/null +++ b/common/autoware_control_center_msgs/srv/Register.srv @@ -0,0 +1,5 @@ +string name_node +--- +autoware_control_center_msgs/NodeStatus status +unique_identifier_msgs/UUID uuid_node +string log_response diff --git a/common/autoware_control_center_msgs/srv/ReportState.srv b/common/autoware_control_center_msgs/srv/ReportState.srv new file mode 100644 index 00000000..7f192b3e --- /dev/null +++ b/common/autoware_control_center_msgs/srv/ReportState.srv @@ -0,0 +1,7 @@ +string name_node +autoware_control_center_msgs/NodeState state +string message +--- +autoware_control_center_msgs/NodeStatus status +unique_identifier_msgs/UUID uuid_node +string log_response 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/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..760c6a42 --- /dev/null +++ b/common/autoware_node/include/autoware/node/node.hpp @@ -0,0 +1,147 @@ +// 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()); + + /*! + 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_strat = + (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_strat); + } + + rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_; + + rclcpp::Client::SharedPtr cli_register_; + + rclcpp::Client::SharedPtr cli_node_error_; + + rclcpp::Publisher::SharedPtr heartbeat_pub_; + + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + + rclcpp::TimerBase::SharedPtr register_timer_; + + bool registered; + + unique_identifier_msgs::msg::UUID self_uuid; + + unique_identifier_msgs::msg::UUID acc_uuid; + + std::string self_name; + +private: + uint16_t sequence_number_; + + void register_callback(); + + void heartbeat_callback(); + using RegisterServiceResponseFuture = + rclcpp::Client::SharedFuture; + + void node_register_future_callback(RegisterServiceResponseFuture future); + using ReportStateServiceResponseFuture = + rclcpp::Client::SharedFuture; + + void node_error_future_callback(ReportStateServiceResponseFuture 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..464432cc --- /dev/null +++ b/common/autoware_node/src/node.cpp @@ -0,0 +1,171 @@ +// 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) +{ + RCLCPP_DEBUG(get_logger(), "Node::Node()"); + declare_parameter( + "heartbeat_period", 200); // TODO(lexavtanke): remove default and add schema + 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()); + std::string self_namespace(this->get_namespace()); + std::string name(this->get_name()); + if (self_namespace.length() > 1) { + self_name = self_namespace + "/" + name; + } else { + self_name = name; + } + sequence_number_ = 0; + registered = false; + + // 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); + + // assert liveliness on the 'heartbeat' topic + heartbeat_pub_ = this->create_publisher( + "~/heartbeat", qos_profile); + heartbeat_timer_ = + this->create_wall_timer(heartbeat_period, std::bind(&Node::heartbeat_callback, this)); + + callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + cli_register_ = create_client( + "/autoware_control_center/srv/autoware_node_register", rmw_qos_profile_default, + callback_group_mut_ex_); + + register_timer_ = + this->create_wall_timer(register_timer_period, std::bind(&Node::register_callback, this)); + + cli_node_error_ = create_client( + "/autoware_control_center/srv/on_report_error", rmw_qos_profile_default, + callback_group_mut_ex_); +} + +void Node::register_callback() +{ + RCLCPP_DEBUG(get_logger(), "Register callback"); + if (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::node_register_future_callback, 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::heartbeat_callback() +{ + 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); + heartbeat_pub_->publish(message); +} + +void Node::send_state( + const autoware_control_center_msgs::msg::NodeState & node_state, std::string message) +{ + if (!cli_node_error_->service_is_ready()) { + RCLCPP_WARN(get_logger(), "%s is unavailable", cli_node_error_->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_node_error_->async_send_request( + req, std::bind(&Node::node_error_future_callback, 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::node_register_future_callback(RegisterServiceResponseFuture 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) { + registered = true; + self_uuid = response->uuid_node; + RCLCPP_DEBUG(get_logger(), "Node was registered"); + register_timer_->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::node_error_future_callback(ReportStateServiceResponseFuture 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..dd30460c --- /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_