From 8af553cf2a204ede9e65bda4393d17f58e87e526 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Mon, 18 Nov 2024 03:05:05 +0300 Subject: [PATCH] update readmes --- common/autoware_control_center/README.md | 15 +++--- common/autoware_node/README.md | 59 +++++++++++++++++++----- common/autoware_node/src/node.cpp | 29 ++++++------ 3 files changed, 70 insertions(+), 33 deletions(-) diff --git a/common/autoware_control_center/README.md b/common/autoware_control_center/README.md index bfd29944b6..c8a811fe04 100644 --- a/common/autoware_control_center/README.md +++ b/common/autoware_control_center/README.md @@ -4,6 +4,7 @@ - **ACC:** Autoware Control Center - **AN:** Autoware Node +- **HB:** Heartbeat ## Overview @@ -53,14 +54,14 @@ This topic also contains the information about the node's status. Publishes reports on the current status of registered Autoware nodes. - **Topic:** `/autoware/control_center/node_reports` -- **Type:** `autoware_control_center_msgs::msg::NodeReport` +- **Type:** `autoware_control_center_msgs::msg::NodeReports` ## 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) | +| Name | Type | Default Value | Description | +|-----------------------|----------|---------------|--------------------------------------------------------------------------| +| `deadline_ms` | `double` | `220.0` | Maximum duration to wait for a heartbeat before considering a node dead. | +| `report_publish_rate` | `double` | `1.0` | Frequency (in Hz) at which NodeReports are published. | ## Singleton Constraint @@ -88,9 +89,9 @@ This ensures that no other instance of ACC is running on any other machine withi 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. +The ACC then subscribes to the heartbeat topic of the Autoware node to monitor its liveliness. -If the ACC does not receive a heartbeat from the Autoware node within the specified lease duration, +If the ACC does not receive a heartbeat from the Autoware node within the specified deadline, it considers the node to be dead. ## Credits diff --git a/common/autoware_node/README.md b/common/autoware_node/README.md index dc3863689b..682fbf13f9 100644 --- a/common/autoware_node/README.md +++ b/common/autoware_node/README.md @@ -1,37 +1,72 @@ # Autoware Node +## Abbreviations + +- **ACC:** Autoware Control Center +- **AN:** Autoware Node +- **HB:** Heartbeat + ## 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) +AN is an `autoware.core` package designed to provide a base class for all future nodes in the +system. +It provides ability to register node to ACC, report node state, publish heartbeats. +It also inherits all lifecycle control capabilities of the base +class [LifecycleNode](https://docs.ros2.org/latest/api/rclcpp_lifecycle/classrclcpp__lifecycle_1_1LifecycleNode.html) ## Usage -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. +Check the [autoware_test_node](../autoware_test_node/README.md) package for an example of how to use `autoware::Node`. ## Design -_Autoware_node_ inherits from ROS 2 [_lifecycle_node_](https://design.ros2.org/articles/node_lifecycle.html) and has all basic functions of it. +### Lifecycle -Below are the main add-ons and how they work. +AN inherits from ROS 2 [rclcpp_lifecycle::LifecycleNode](https://design.ros2.org/articles/node_lifecycle.html) and has +all the basic functions of it. ### Registration -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. +Relevant parameters: + +| Parameter | Function | +|----------------------------|------------------------------------------------------------------------| +| `period_timer_register_ms` | The interval at which the AN periodically attempts to register itself. | + +Upon startup, AN starts a registration timer and periodically attempts to register itself to ACC via a service call. +This registration timer runs asynchronously to the rest of the node's functionality. -### De-registration +Upon successful 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. +- The registration timer is stopped. +- ACC will create a subscription to the HB messages of AN. +- ACC will return a UUID specific to the AN instance. ### Error state -_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. +WIP. +Currently, only sends positive heartbeat messages. -### Heartbeat +### Heartbeat (**HB**) -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. +Relevant parameters: -Heartbeat functionality is based on ros2 [software_watchdogs](https://github.com/ros-safety/software_watchdogs) package. +| Parameter | Function | +|-----------------------|------------------------------------------------------------------------| +| `deadline_ms` | If ACC doesn't receive a HB by this deadline, AN will be assumed dead. | +| `period_heartbeat_ms` | AN is expected to publish a HB with this period. | + +* 🟡 `deadline_ms` should be slightly higher than `period_heartbeat_ms` to account for network delays. +* 🔴 `deadline_ms` of AN should match the `deadline_ms` of ACC. + +Upon registration, AN starts a timer that periodically publishes HB messages to ACC. +This HB timer runs asynchronously to the rest of the node's functionality. ### Monitored subscription -_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. +WIP. + +## Credits + +- Heartbeat functionality is based on ROS 2 [software_watchdogs](https://github.com/ros-safety/software_watchdogs) + package. diff --git a/common/autoware_node/src/node.cpp b/common/autoware_node/src/node.cpp index fc4c41d6d2..3413d76009 100644 --- a/common/autoware_node/src/node.cpp +++ b/common/autoware_node/src/node.cpp @@ -26,35 +26,36 @@ namespace autoware::node { Node::Node( const std::string & node_name, const std::string & ns, const rclcpp::NodeOptions & options) -: LifecycleNode(node_name, ns, options), - sequence_number_{0}, - is_registered_{false}, - period_timer_register_ms_{declare_parameter("period_timer_register_ms")}, - period_heartbeat_ms_{declare_parameter("period_heartbeat_ms")}, - deadline_ms_{declare_parameter("deadline_ms")} + : LifecycleNode(node_name, ns, options), + sequence_number_{0}, + is_registered_{false}, + period_timer_register_ms_{declare_parameter("period_timer_register_ms")}, + period_heartbeat_ms_{declare_parameter("period_heartbeat_ms")}, + deadline_ms_{declare_parameter("deadline_ms")} { RCLCPP_DEBUG( get_logger(), "Node %s constructor", get_node_base_interface()->get_fully_qualified_name()); rclcpp::QoS qos_profile(1); qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) - .liveliness_lease_duration(std::chrono::duration(deadline_ms_)) - .deadline(std::chrono::duration(deadline_ms_)); + .liveliness_lease_duration(std::chrono::duration(deadline_ms_)) + .deadline(std::chrono::duration(deadline_ms_)); pub_heartbeat_ = this->create_publisher( "~/heartbeat", qos_profile); - timer_heartbeat_ = this->create_wall_timer( - std::chrono::duration(period_heartbeat_ms_), - std::bind(&Node::on_tick_heartbeat, this)); callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_heartbeat_ = this->create_wall_timer( + std::chrono::duration(period_heartbeat_ms_), + std::bind(&Node::on_tick_heartbeat, this), callback_group_mut_ex_); + cli_register_ = create_client( - "/autoware/control_center/srv/register", rmw_qos_profile_default); + "/autoware/control_center/srv/register", rmw_qos_profile_default, callback_group_mut_ex_); timer_registration_ = this->create_wall_timer( std::chrono::duration(period_timer_register_ms_), - std::bind(&Node::on_tick_registration, this)); + std::bind(&Node::on_tick_registration, this), callback_group_mut_ex_); } void Node::on_tick_heartbeat() @@ -107,4 +108,4 @@ void Node::on_register(FutureRegister future) is_registered_ = true; RCLCPP_DEBUG(get_logger(), "Registration succeeded."); } -} // namespace autoware::node +} // namespace autoware::node \ No newline at end of file