From a5862709ea5e81553a217be9d6fe694dda1be2f9 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Mon, 29 Jan 2024 19:45:05 +0300 Subject: [PATCH] feat(autoware_control_center): implement AutowareNodeError srv in ACC Signed-off-by: Alexey Panferov --- .../autoware_control_center.hpp | 14 +++++++ .../src/autoware_control_center.cpp | 39 ++++++++++++++++++- 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp b/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp index 3e4f40e5..b5ef7005 100644 --- a/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp +++ b/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE_CONTROL_CENTER__AUTOWARE_CONTROL_CENTER_HPP_ #define AUTOWARE_CONTROL_CENTER__AUTOWARE_CONTROL_CENTER_HPP_ + #include "autoware_control_center/node_registry.hpp" #include "autoware_control_center/visibility_control.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -23,15 +24,23 @@ #include "autoware_control_center_msgs/msg/heartbeat.hpp" #include "autoware_control_center_msgs/srv/autoware_node_deregister.hpp" #include "autoware_control_center_msgs/srv/autoware_node_register.hpp" +#include "autoware_control_center_msgs/srv/autoware_node_error.hpp" + +#include +#include namespace autoware_control_center { unique_identifier_msgs::msg::UUID createDefaultUUID(); +enum class HealthState {Unknown = 0, Healthy = 1, Warning = 2, Error = 3}; + struct AutowareNodeStatus { bool alive; rclcpp::Time last_heartbeat; + std::string node_report; + autoware_control_center_msgs::msg::AutowareNodeState state; }; class AutowareControlCenter : public rclcpp_lifecycle::LifecycleNode @@ -45,6 +54,7 @@ class AutowareControlCenter : public rclcpp_lifecycle::LifecycleNode rclcpp::Service::SharedPtr srv_register_; rclcpp::Service::SharedPtr srv_deregister_; + rclcpp::Service::SharedPtr srv_node_error_; rclcpp::Publisher::SharedPtr node_reports_pub_; NodeRegistry node_registry_; @@ -68,6 +78,10 @@ class AutowareControlCenter : public rclcpp_lifecycle::LifecycleNode void deregister_node( const autoware_control_center_msgs::srv::AutowareNodeDeregister::Request::SharedPtr request, const autoware_control_center_msgs::srv::AutowareNodeDeregister::Response::SharedPtr response); + + void autoware_node_error( + const autoware_control_center_msgs::srv::AutowareNodeError::Request::SharedPtr request, + const autoware_control_center_msgs::srv::AutowareNodeError::Response::SharedPtr response); void startup_callback(); diff --git a/common/autoware_control_center/src/autoware_control_center.cpp b/common/autoware_control_center/src/autoware_control_center.cpp index 2d38d5d9..e5520592 100644 --- a/common/autoware_control_center/src/autoware_control_center.cpp +++ b/common/autoware_control_center/src/autoware_control_center.cpp @@ -58,6 +58,9 @@ AutowareControlCenter::AutowareControlCenter(const rclcpp::NodeOptions & options "~/srv/autoware_node_deregister", std::bind(&AutowareControlCenter::deregister_node, this, _1, _2), rmw_qos_profile_services_default, callback_group_mut_ex_); + srv_node_error_ = create_service( + "~/srv/autoware_node_error", std::bind(&AutowareControlCenter::autoware_node_error, this, _1, _2), + rmw_qos_profile_services_default, callback_group_mut_ex_); node_reports_pub_ = create_publisher( "~/autoware_node_reports", 1); @@ -211,20 +214,52 @@ AutowareControlCenter::create_heartbeat_sub(const std::string & node_name) node_status_map_[node_name].last_heartbeat = msg->stamp; }, heartbeat_sub_options_); - node_status_map_.insert({node_name, {false, this->now()}}); + // alive, last_heartbeat, node_report, state + autoware_control_center_msgs::msg::AutowareNodeState un_state; + un_state.status = autoware_control_center_msgs::msg::AutowareNodeState::UNKNOWN; + node_status_map_.insert({ + node_name, { + false, + this->now(), + "", + un_state + } + }); return heartbeat_sub_; } void AutowareControlCenter::node_reports_callback() { autoware_control_center_msgs::msg::AutowareNodeReports msg; + rclcpp::Time stamp = this->now(); + msg.stamp = stamp; for (auto const & [name, info] : node_status_map_) { autoware_control_center_msgs::msg::AutowareNodeReport report; report.name_node = name; report.alive = info.alive; - report.last_heartbeat = this->now() - info.last_heartbeat; + report.last_heartbeat = stamp - info.last_heartbeat; msg.nodes.push_back(report); } node_reports_pub_->publish(msg); } + +void AutowareControlCenter::autoware_node_error( + const autoware_control_center_msgs::srv::AutowareNodeError::Request::SharedPtr request, + const autoware_control_center_msgs::srv::AutowareNodeError::Response::SharedPtr response) +{ + RCLCPP_INFO(get_logger(), "autoware_node_error 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 = autoware_control_center_msgs::srv::AutowareNodeError::Response::_status_type::SUCCESS; + response->uuid_node = node_registry_.get_uuid(request->name_node).value(); + } else { + response->status.status = autoware_control_center_msgs::srv::AutowareNodeError::Response::_status_type::FAILURE; + response->uuid_node = createDefaultUUID(); + response->log_response = request->name_node + " node was not registered."; + } +} + } // namespace autoware_control_center