feat(default_ad_api): add diagnostics api (autowarefoundation#7052)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Author: isamu-takagi

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/heartbeat.cpp
   src/interface.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::DiagnosticsNode"
   "default_ad_api::FailSafeNode"
   "default_ad_api::HeartbeatNode"
   "default_ad_api::InterfaceNode"

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"),

system/default_ad_api/package.xml

@@ -21,6 +21,7 @@
   <depend>autoware_planning_msgs</depend>
   <depend>component_interface_specs</depend>
   <depend>component_interface_utils</depend>
+  <depend>diagnostic_graph_utils</depend>
   <depend>geographic_msgs</depend>
   <depend>geography_utils</depend>
   <depend>motion_utils</depend>

system/default_ad_api/src/diagnostics.cpp

@@ -0,0 +1,81 @@
+// 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 <memory>
+#include <unordered_map>
+
+namespace default_ad_api
+{
+
+DiagnosticsNode::DiagnosticsNode(const rclcpp::NodeOptions & options) : Node("diagnostics", options)
+{
+  using std::placeholders::_1;
+
+  pub_struct_ = create_publisher<autoware_adapi_v1_msgs::msg::DiagGraphStruct>(
+    "/api/system/diagnostics/struct", rclcpp::QoS(1).transient_local());
+  pub_status_ = create_publisher<autoware_adapi_v1_msgs::msg::DiagGraphStatus>(
+    "/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)
+{
+  const auto & units = graph->units();
+  const auto & links = graph->links();
+
+  std::unordered_map<DiagUnit *, size_t> 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)
+{
+  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_status_->publish(msg);
+}
+
+}  // namespace default_ad_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::DiagnosticsNode)

system/default_ad_api/src/diagnostics.hpp

@@ -0,0 +1,46 @@
+// 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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/diag_graph_status.hpp>
+#include <autoware_adapi_v1_msgs/msg/diag_graph_struct.hpp>
+
+namespace default_ad_api
+{
+
+class DiagnosticsNode : public rclcpp::Node
+{
+public:
+  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<autoware_adapi_v1_msgs::msg::DiagGraphStruct>::SharedPtr pub_struct_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::DiagGraphStatus>::SharedPtr pub_status_;
+  diagnostic_graph_utils::DiagGraphSubscription sub_graph_;
+};
+
+}  // namespace default_ad_api
+
+#endif  // DIAGNOSTICS_HPP_

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<DiagUnit *> units() const;
   std::vector<DiagNode *> nodes() const;
   std::vector<DiagLeaf *> diags() const;

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<DiagLink>(data)).get();
+    const auto link = links_.emplace_back(std::make_unique<DiagLink>(data, parent, child)).get();
     parent->add_child({link, child});
   }
 }