Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(default_ad_api): add diagnostics api #7052

Merged
2 changes: 2 additions & 0 deletions system/default_ad_api/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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"
Expand Down
1 change: 1 addition & 0 deletions system/default_ad_api/launch/default_ad_api.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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"),
Expand Down
1 change: 1 addition & 0 deletions system/default_ad_api/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
81 changes: 81 additions & 0 deletions system/default_ad_api/src/diagnostics.cpp
Original file line number Diff line number Diff line change
@@ -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)
46 changes: 46 additions & 0 deletions system/default_ad_api/src/diagnostics.hpp
Original file line number Diff line number Diff line change
@@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion system/diagnostic_graph_utils/src/lib/graph.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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});
}
}
Expand Down
Loading