From eb60bb153e8903a0661d0e875a4a80a5e2e2e8b7 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 8 May 2024 21:02:25 +0900 Subject: [PATCH 1/8] feat(default_ad_api): add heratbeat api Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/system.hpp | 36 ++++++++++++++++ system/default_ad_api/CMakeLists.txt | 2 + .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/src/heartbeat.cpp | 41 +++++++++++++++++++ system/default_ad_api/src/heartbeat.hpp | 40 ++++++++++++++++++ 5 files changed, 120 insertions(+) create mode 100644 common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp create mode 100644 system/default_ad_api/src/heartbeat.cpp create mode 100644 system/default_ad_api/src/heartbeat.hpp diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp new file mode 100644 index 0000000000000..09144c1d8ff50 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp @@ -0,0 +1,36 @@ +// 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_AD_API_SPECS__SYSTEM_HPP_ +#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ + +#include + +#include + +namespace autoware_ad_api::system +{ + +struct Heartbeat +{ + using Message = autoware_adapi_v1_msgs::msg::Heartbeat; + static constexpr char name[] = "/api/system/heartbeat"; + static constexpr size_t depth = 10; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::system + +#endif // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 982112189412e..4dacc0b893714 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/fail_safe.cpp + src/heartbeat.cpp src/interface.cpp src/localization.cpp src/motion.cpp @@ -23,6 +24,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::AutowareStateNode" "default_ad_api::FailSafeNode" + "default_ad_api::HeartbeatNode" "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index cf8fbe7d001bf..f4f3986c678e9 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -43,6 +43,7 @@ def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), create_api_node("fail_safe", "FailSafeNode"), + create_api_node("heartbeat", "HeartbeatNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/default_ad_api/src/heartbeat.cpp new file mode 100644 index 0000000000000..74766e24940c9 --- /dev/null +++ b/system/default_ad_api/src/heartbeat.cpp @@ -0,0 +1,41 @@ +// 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 "heartbeat.hpp" + +#include + +namespace default_ad_api +{ + +HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) +{ + auto on_timer = [this]() { + autoware_ad_api::system::Heartbeat::Message heartbeat; + heartbeat.stamp = now(); + heartbeat.seq = ++sequence_; // Wraps at 65535. + pub_->publish(heartbeat); + }; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_); + + const auto period = rclcpp::Rate(10.0).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, on_timer); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::HeartbeatNode) diff --git a/system/default_ad_api/src/heartbeat.hpp b/system/default_ad_api/src/heartbeat.hpp new file mode 100644 index 0000000000000..d922b88489639 --- /dev/null +++ b/system/default_ad_api/src/heartbeat.hpp @@ -0,0 +1,40 @@ +// 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 HEARTBEAT_HPP_ +#define HEARTBEAT_HPP_ + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class HeartbeatNode : public rclcpp::Node +{ +public: + explicit HeartbeatNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::TimerBase::SharedPtr timer_; + Pub pub_; + uint16_t sequence_ = 0; +}; + +} // namespace default_ad_api + +#endif // HEARTBEAT_HPP_ From 2d5b977c80a8f08473114d76fdfc168ae4d83d24 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 10 May 2024 21:44:12 +0900 Subject: [PATCH 2/8] add node Signed-off-by: Takagi, Isamu --- system/default_ad_api/CMakeLists.txt | 1 + .../launch/default_ad_api.launch.xml | 9 ++++ system/default_ad_api/package.xml | 1 + system/default_ad_api/src/diagnostics.cpp | 52 +++++++++++++++++++ system/default_ad_api/src/diagnostics.hpp | 44 ++++++++++++++++ 5 files changed, 107 insertions(+) create mode 100644 system/default_ad_api/launch/default_ad_api.launch.xml create mode 100644 system/default_ad_api/src/diagnostics.cpp create mode 100644 system/default_ad_api/src/diagnostics.hpp diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 982112189412e..bf3389ba9afdb 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED + src/diagnostics.cpp src/fail_safe.cpp src/interface.cpp src/localization.cpp diff --git a/system/default_ad_api/launch/default_ad_api.launch.xml b/system/default_ad_api/launch/default_ad_api.launch.xml new file mode 100644 index 0000000000000..4c3661522ecc2 --- /dev/null +++ b/system/default_ad_api/launch/default_ad_api.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 4e691f7ab3192..f9b5a167bdab1 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -21,6 +21,7 @@ autoware_planning_msgs component_interface_specs component_interface_utils + diagnostic_graph_utils geographic_msgs geography_utils motion_utils diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/default_ad_api/src/diagnostics.cpp new file mode 100644 index 0000000000000..d26144e693823 --- /dev/null +++ b/system/default_ad_api/src/diagnostics.cpp @@ -0,0 +1,52 @@ +// 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 "diagnostics.hpp" + +#include + +namespace default_ad_api +{ + +DiagnosticsNode::DiagnosticsNode() : Node("diagnostics") +{ + using std::placeholders::_1; + pub_struct_ = create_publisher( + "/api/diagnostics/struct", rclcpp::QoS(1)); + pub_status_ = create_publisher( + "/api/diagnostics/status", rclcpp::QoS(1)); + sub_graph_.register_create_callback(std::bind(&DiagnosticsNode::on_create, this, _1)); + sub_graph_.register_update_callback(std::bind(&DiagnosticsNode::on_update, this, _1)); + sub_graph_.subscribe(*this, 10); +} +void DiagnosticsNode::on_create(DiagGraph::ConstSharedPtr graph) +{ + (void)graph; +} + +void DiagnosticsNode::on_update(DiagGraph::ConstSharedPtr graph) +{ + (void)graph; + /* + DiagnosticArray array; + array.header.stamp = graph->updated_stamp(); + for (const auto & unit : graph->units()) { + if (unit->path().empty()) continue; + array.status.push_back(unit->create_diagnostic_status()); + } + pub_array_->publish(array); + */ +} + +} // namespace default_ad_api diff --git a/system/default_ad_api/src/diagnostics.hpp b/system/default_ad_api/src/diagnostics.hpp new file mode 100644 index 0000000000000..540f0bfc32b7b --- /dev/null +++ b/system/default_ad_api/src/diagnostics.hpp @@ -0,0 +1,44 @@ +// 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 DIAGNOSTICS_HPP_ +#define DIAGNOSTICS_HPP_ + +#include "diagnostic_graph_utils/subscription.hpp" + +#include + +#include +#include + +namespace default_ad_api +{ + +class DiagnosticsNode : public rclcpp::Node +{ +public: + DiagnosticsNode(); + +private: + using DiagGraph = diagnostic_graph_utils::DiagGraph; + void on_create(DiagGraph::ConstSharedPtr graph); + void on_update(DiagGraph::ConstSharedPtr graph); + rclcpp::Publisher::SharedPtr pub_struct_; + rclcpp::Publisher::SharedPtr pub_status_; + diagnostic_graph_utils::DiagGraphSubscription sub_graph_; +}; + +} // namespace default_ad_api + +#endif // DIAGNOSTICS_HPP_ From b3161cbc1526c57a8340fbb073502214901b1997 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 15 May 2024 18:40:48 +0900 Subject: [PATCH 3/8] fix node dying Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/heartbeat.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/default_ad_api/src/heartbeat.cpp index 74766e24940c9..4550302bb8bae 100644 --- a/system/default_ad_api/src/heartbeat.cpp +++ b/system/default_ad_api/src/heartbeat.cpp @@ -21,7 +21,8 @@ namespace default_ad_api HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) { - auto on_timer = [this]() { + // Move this function so that the timer no longer holds it as a reference. + const auto on_timer = [this]() { autoware_ad_api::system::Heartbeat::Message heartbeat; heartbeat.stamp = now(); heartbeat.seq = ++sequence_; // Wraps at 65535. @@ -32,7 +33,7 @@ HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartb adaptor.init_pub(pub_); const auto period = rclcpp::Rate(10.0).period(); - timer_ = rclcpp::create_timer(this, get_clock(), period, on_timer); + timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(on_timer)); } } // namespace default_ad_api From c91add62d7b95de646ed9b19e8c72bb699a0fc87 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 15 May 2024 19:43:59 +0900 Subject: [PATCH 4/8] update launch args Signed-off-by: Takagi, Isamu --- launch/tier4_system_launch/launch/system.launch.xml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index a1bbfc883de7c..1574789d2bed5 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -28,8 +28,8 @@ - - + + @@ -118,12 +118,12 @@ - - - + + + - + From e8f36504fc874d24915d880bb40e092829ec15b5 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 17 May 2024 12:01:25 +0900 Subject: [PATCH 5/8] update launch files Signed-off-by: Takagi, Isamu --- .../launch/system.launch.xml | 72 ++++++++++--------- .../system_diagnostic_monitor.launch.xml | 2 + 2 files changed, 39 insertions(+), 35 deletions(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 1574789d2bed5..e5f7aa4e18b33 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -25,11 +25,12 @@ - - - - - + + + + + + @@ -57,6 +58,13 @@ + + + + + + + @@ -70,8 +78,20 @@ + + + + + + + + + + + + - + @@ -85,52 +105,34 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - + - + - + + + + + + + + diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml index b00fcb8a26f68..8e2566a747abf 100644 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml @@ -1,6 +1,8 @@ + + From 6a3ced589df4f3887ecb7cd515285e457259296a Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 17 May 2024 13:52:42 +0900 Subject: [PATCH 6/8] use operation move availability Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/operation_mode.cpp | 40 +++++--------------- system/default_ad_api/src/operation_mode.hpp | 8 ++-- 2 files changed, 13 insertions(+), 35 deletions(-) diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 829585ed4b8b4..d4b430ecd0579 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -39,18 +39,15 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) adaptor.init_cli(cli_mode_, group_cli_); adaptor.init_cli(cli_control_, group_cli_); - const std::vector module_names = { - "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + const auto name = "/system/operation_mode/availability"; + const auto qos = rclcpp::QoS(1); + const auto callback = [this](const OperationModeAvailability::ConstSharedPtr msg) { + mode_available_[OperationModeState::Message::STOP] = msg->stop; + mode_available_[OperationModeState::Message::AUTONOMOUS] = msg->autonomous; + mode_available_[OperationModeState::Message::LOCAL] = msg->local; + mode_available_[OperationModeState::Message::REMOTE] = msg->remote; }; - - for (size_t i = 0; i < module_names.size(); ++i) { - const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; - const auto qos = rclcpp::QoS(1).transient_local(); - const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) { - module_states_[module_names[i]] = msg->available; - }; - sub_module_states_.push_back(create_subscription(name, qos, callback)); - } + sub_availability_ = create_subscription(name, qos, callback); timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); @@ -60,8 +57,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) mode_available_[OperationModeState::Message::UNKNOWN] = false; mode_available_[OperationModeState::Message::STOP] = true; mode_available_[OperationModeState::Message::AUTONOMOUS] = false; - mode_available_[OperationModeState::Message::LOCAL] = true; - mode_available_[OperationModeState::Message::REMOTE] = true; + mode_available_[OperationModeState::Message::LOCAL] = false; + mode_available_[OperationModeState::Message::REMOTE] = false; } template @@ -135,23 +132,6 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { - bool autonomous_available = true; - std::string unhealthy_components = ""; - for (const auto & state : module_states_) { - if (!state.second) { - unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first; - } - autonomous_available &= state.second; - } - mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; - - if (!unhealthy_components.empty()) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 3000, - "%s component state is unhealthy. Autonomous is not available.", - unhealthy_components.c_str()); - } - update_state(); } diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 1830b7041b566..7cd609b5eb852 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -25,7 +25,7 @@ #include // TODO(Takagi, Isamu): define interface -#include +#include // This file should be included after messages. #include "utils/types.hpp" @@ -47,7 +47,7 @@ class OperationModeNode : public rclcpp::Node using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; - using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; + using OperationModeAvailability = tier4_system_msgs::msg::OperationModeAvailability; OperationModeState::Message curr_state_; OperationModeState::Message prev_state_; @@ -65,9 +65,7 @@ class OperationModeNode : public rclcpp::Node Sub sub_state_; Cli cli_mode_; Cli cli_control_; - - std::unordered_map module_states_; - std::vector::SharedPtr> sub_module_states_; + rclcpp::Subscription::SharedPtr sub_availability_; void on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr req, From 84bdfbb217f6a15c21b1cd7e15bcdc14a43f8893 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 17 May 2024 19:46:17 +0900 Subject: [PATCH 7/8] add diagnostics api --- system/default_ad_api/CMakeLists.txt | 1 + .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/src/diagnostics.cpp | 55 ++++++++++++++----- system/default_ad_api/src/diagnostics.hpp | 4 +- .../include/diagnostic_graph_utils/graph.hpp | 11 +++- .../diagnostic_graph_utils/src/lib/graph.cpp | 2 +- 6 files changed, 58 insertions(+), 16 deletions(-) diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 2cf85cc3c06e1..7975937f5b2c1 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -24,6 +24,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::AutowareStateNode" + "default_ad_api::DiagnosticsNode" "default_ad_api::FailSafeNode" "default_ad_api::HeartbeatNode" "default_ad_api::InterfaceNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index f4f3986c678e9..ca1575a528db2 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -42,6 +42,7 @@ def get_default_config(): def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), + create_api_node("diagnostics", "DiagnosticsNode"), create_api_node("fail_safe", "FailSafeNode"), create_api_node("heartbeat", "HeartbeatNode"), create_api_node("interface", "InterfaceNode"), diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/default_ad_api/src/diagnostics.cpp index d26144e693823..f4e58b5a4d441 100644 --- a/system/default_ad_api/src/diagnostics.cpp +++ b/system/default_ad_api/src/diagnostics.cpp @@ -15,38 +15,67 @@ #include "diagnostics.hpp" #include +#include namespace default_ad_api { -DiagnosticsNode::DiagnosticsNode() : Node("diagnostics") +DiagnosticsNode::DiagnosticsNode(const rclcpp::NodeOptions & options) : Node("diagnostics", options) { using std::placeholders::_1; + pub_struct_ = create_publisher( - "/api/diagnostics/struct", rclcpp::QoS(1)); + "/api/system/diagnostics/struct", rclcpp::QoS(1).transient_local()); pub_status_ = create_publisher( - "/api/diagnostics/status", rclcpp::QoS(1)); + "/api/system/diagnostics/status", rclcpp::QoS(1)); + sub_graph_.register_create_callback(std::bind(&DiagnosticsNode::on_create, this, _1)); sub_graph_.register_update_callback(std::bind(&DiagnosticsNode::on_update, this, _1)); sub_graph_.subscribe(*this, 10); } void DiagnosticsNode::on_create(DiagGraph::ConstSharedPtr graph) { - (void)graph; + const auto & units = graph->units(); + const auto & links = graph->links(); + + std::unordered_map unit_indices_; + for (size_t i = 0; i < units.size(); ++i) { + unit_indices_[units[i]] = i; + } + + autoware_adapi_v1_msgs::msg::DiagGraphStruct msg; + msg.stamp = graph->created_stamp(); + msg.id = graph->id(); + msg.nodes.reserve(units.size()); + msg.links.reserve(links.size()); + for (const auto & unit : units) { + msg.nodes.emplace_back(); + msg.nodes.back().path = unit->path(); + } + for (const auto & link : links) { + msg.links.emplace_back(); + msg.links.back().parent = unit_indices_.at(link->parent()); + msg.links.back().child = unit_indices_.at(link->child()); + } + pub_struct_->publish(msg); } void DiagnosticsNode::on_update(DiagGraph::ConstSharedPtr graph) { - (void)graph; - /* - DiagnosticArray array; - array.header.stamp = graph->updated_stamp(); - for (const auto & unit : graph->units()) { - if (unit->path().empty()) continue; - array.status.push_back(unit->create_diagnostic_status()); + const auto & units = graph->units(); + + autoware_adapi_v1_msgs::msg::DiagGraphStatus msg; + msg.stamp = graph->updated_stamp(); + msg.id = graph->id(); + msg.nodes.reserve(units.size()); + for (const auto & unit : units) { + msg.nodes.emplace_back(); + msg.nodes.back().level = unit->level(); } - pub_array_->publish(array); - */ + pub_status_->publish(msg); } } // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::DiagnosticsNode) diff --git a/system/default_ad_api/src/diagnostics.hpp b/system/default_ad_api/src/diagnostics.hpp index 540f0bfc32b7b..302ffe39159df 100644 --- a/system/default_ad_api/src/diagnostics.hpp +++ b/system/default_ad_api/src/diagnostics.hpp @@ -28,10 +28,12 @@ namespace default_ad_api class DiagnosticsNode : public rclcpp::Node { public: - DiagnosticsNode(); + explicit DiagnosticsNode(const rclcpp::NodeOptions & options); private: using DiagGraph = diagnostic_graph_utils::DiagGraph; + using DiagUnit = diagnostic_graph_utils::DiagUnit; + using DiagLink = diagnostic_graph_utils::DiagLink; void on_create(DiagGraph::ConstSharedPtr graph); void on_update(DiagGraph::ConstSharedPtr graph); rclcpp::Publisher::SharedPtr pub_struct_; diff --git a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp index b2c6fc752e463..c5b386dbe2c86 100644 --- a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp +++ b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp @@ -55,12 +55,20 @@ class DiagLink public: using DiagLinkStruct = tier4_system_msgs::msg::DiagLinkStruct; using DiagLinkStatus = tier4_system_msgs::msg::DiagLinkStatus; - explicit DiagLink(const DiagLinkStruct & msg) : struct_(msg) {} + DiagLink(const DiagLinkStruct & msg, DiagUnit * parent, DiagUnit * child) : struct_(msg) + { + parent_ = parent; + child_ = child; + } void update(const DiagLinkStatus & msg) { status_ = msg; } + DiagUnit * parent() const { return parent_; } + DiagUnit * child() const { return child_; } private: DiagLinkStruct struct_; DiagLinkStatus status_; + DiagUnit * parent_; + DiagUnit * child_; }; class DiagNode : public DiagUnit @@ -114,6 +122,7 @@ class DiagGraph bool update(const DiagGraphStatus & msg); rclcpp::Time created_stamp() const { return created_stamp_; } rclcpp::Time updated_stamp() const { return updated_stamp_; } + std::string id() const { return id_; } std::vector units() const; std::vector nodes() const; std::vector diags() const; diff --git a/system/diagnostic_graph_utils/src/lib/graph.cpp b/system/diagnostic_graph_utils/src/lib/graph.cpp index f1177c1047bdb..bd4c215a346ed 100644 --- a/system/diagnostic_graph_utils/src/lib/graph.cpp +++ b/system/diagnostic_graph_utils/src/lib/graph.cpp @@ -54,7 +54,7 @@ void DiagGraph::create(const DiagGraphStruct & msg) for (const auto & data : msg.links) { DiagNode * parent = nodes_.at(data.parent).get(); DiagUnit * child = get_child(data.is_leaf, data.child); - const auto link = links_.emplace_back(std::make_unique(data)).get(); + const auto link = links_.emplace_back(std::make_unique(data, parent, child)).get(); parent->add_child({link, child}); } } From c9bf85a859c63b433fb9c48df7f3da3b88d99aea Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 17 May 2024 19:47:05 +0900 Subject: [PATCH 8/8] remove debug file Signed-off-by: Takagi, Isamu --- system/default_ad_api/launch/default_ad_api.launch.xml | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 system/default_ad_api/launch/default_ad_api.launch.xml diff --git a/system/default_ad_api/launch/default_ad_api.launch.xml b/system/default_ad_api/launch/default_ad_api.launch.xml deleted file mode 100644 index 4c3661522ecc2..0000000000000 --- a/system/default_ad_api/launch/default_ad_api.launch.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - -