Skip to content

Commit 5d17128

Browse files
isamu-takagiisamu-takagi
authored
feat(hazard_status_converter): add package (autowarefoundation#6428)
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent bad6680 commit 5d17128

File tree

5 files changed

+251
-0
lines changed

5 files changed

+251
-0
lines changed

‎system/hazard_status_converter/CMakeLists.txt

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(hazard_status_converter)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(${PROJECT_NAME} SHARED
8+
src/converter.cpp
9+
)
10+
11+
rclcpp_components_register_node(${PROJECT_NAME}
12+
PLUGIN "hazard_status_converter::Converter"
13+
EXECUTABLE converter
14+
)
15+
16+
ament_auto_package(INSTALL_TO_SHARE launch)

‎system/hazard_status_converter/launch/hazard_status_converter.launch.xml

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
<launch>
2+
<node pkg="hazard_status_converter" exec="converter" name="hazard_status_converter"/>
3+
</launch>

‎system/hazard_status_converter/package.xml

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>hazard_status_converter</name>
5+
<version>0.1.0</version>
6+
<description>The hazard_status_converter package</description>
7+
<maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
11+
<buildtool_depend>autoware_cmake</buildtool_depend>
12+
13+
<depend>autoware_auto_system_msgs</depend>
14+
<depend>diagnostic_msgs</depend>
15+
<depend>rclcpp</depend>
16+
<depend>rclcpp_components</depend>
17+
<depend>tier4_system_msgs</depend>
18+
19+
<test_depend>ament_lint_auto</test_depend>
20+
<test_depend>autoware_lint_common</test_depend>
21+
22+
<export>
23+
<build_type>ament_cmake</build_type>
24+
</export>
25+
</package>

‎system/hazard_status_converter/src/converter.cpp

+162
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,162 @@
1+
// Copyright 2023 The Autoware Contributors
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "converter.hpp"
16+
17+
#include <utility>
18+
#include <vector>
19+
20+
namespace
21+
{
22+
23+
using autoware_auto_system_msgs::msg::HazardStatus;
24+
using autoware_auto_system_msgs::msg::HazardStatusStamped;
25+
using diagnostic_msgs::msg::DiagnosticStatus;
26+
using tier4_system_msgs::msg::DiagnosticGraph;
27+
using tier4_system_msgs::msg::DiagnosticNode;
28+
using DiagnosticLevel = DiagnosticStatus::_level_type;
29+
30+
enum class HazardLevel { NF, SF, LF, SPF };
31+
32+
struct TempNode
33+
{
34+
const DiagnosticNode & node;
35+
bool is_auto_tree;
36+
};
37+
38+
HazardLevel get_hazard_level(const TempNode & node, DiagnosticLevel auto_mode_level)
39+
{
40+
// Convert the level according to the table below.
41+
// The Level other than auto mode is considered OK.
42+
// |-------|-------------------------------|
43+
// | Level | Root level |
44+
// |-------|-------------------------------|
45+
// | | OK | WARN | ERROR | STALE |
46+
// | OK | NF | NF | NF | NF |
47+
// | WARN | SF | LF | LF | LF |
48+
// | ERROR | SF | LF | SPF | SPF |
49+
// | STALE | SF | LF | SPF | SPF |
50+
// |-------|-------------------------------|
51+
52+
const auto root_level = node.is_auto_tree ? auto_mode_level : DiagnosticStatus::OK;
53+
const auto node_level = node.node.status.level;
54+
55+
if (node_level == DiagnosticStatus::OK) {
56+
return HazardLevel::NF;
57+
}
58+
if (root_level == DiagnosticStatus::OK) {
59+
return HazardLevel::SF;
60+
}
61+
if (node_level == DiagnosticStatus::WARN) {
62+
return HazardLevel::LF;
63+
}
64+
if (root_level == DiagnosticStatus::WARN) {
65+
return HazardLevel::LF;
66+
}
67+
return HazardLevel::SPF;
68+
}
69+
70+
void set_auto_tree(std::vector<TempNode> & nodes, int index)
71+
{
72+
TempNode & node = nodes[index];
73+
if (node.is_auto_tree) {
74+
return;
75+
}
76+
77+
node.is_auto_tree = true;
78+
for (const auto & link : node.node.links) {
79+
set_auto_tree(nodes, link.index);
80+
}
81+
}
82+
83+
HazardStatusStamped convert_hazard_diagnostics(const DiagnosticGraph & graph)
84+
{
85+
// Create temporary tree for conversion.
86+
std::vector<TempNode> nodes;
87+
nodes.reserve(graph.nodes.size());
88+
for (const auto & node : graph.nodes) {
89+
nodes.push_back({node, false});
90+
}
91+
92+
// Mark nodes included in the auto mode tree.
93+
DiagnosticLevel auto_mode_level = DiagnosticStatus::STALE;
94+
for (size_t index = 0; index < nodes.size(); ++index) {
95+
const auto & status = nodes[index].node.status;
96+
if (status.name == "/autoware/modes/autonomous") {
97+
set_auto_tree(nodes, index);
98+
auto_mode_level = status.level;
99+
}
100+
}
101+
102+
// Calculate hazard level from node level and root level.
103+
HazardStatusStamped hazard;
104+
for (const auto & node : nodes) {
105+
switch (get_hazard_level(node, auto_mode_level)) {
106+
case HazardLevel::NF:
107+
hazard.status.diag_no_fault.push_back(node.node.status);
108+
break;
109+
case HazardLevel::SF:
110+
hazard.status.diag_safe_fault.push_back(node.node.status);
111+
break;
112+
case HazardLevel::LF:
113+
hazard.status.diag_latent_fault.push_back(node.node.status);
114+
break;
115+
case HazardLevel::SPF:
116+
hazard.status.diag_single_point_fault.push_back(node.node.status);
117+
break;
118+
}
119+
}
120+
return hazard;
121+
}
122+
123+
} // namespace
124+
125+
namespace hazard_status_converter
126+
{
127+
128+
Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", options)
129+
{
130+
pub_hazard_ = create_publisher<HazardStatusStamped>("/hazard_status", rclcpp::QoS(1));
131+
sub_graph_ = create_subscription<DiagnosticGraph>(
132+
"/diagnostics_graph", rclcpp::QoS(3),
133+
std::bind(&Converter::on_graph, this, std::placeholders::_1));
134+
}
135+
136+
void Converter::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
137+
{
138+
const auto get_system_level = [](const HazardStatus & status) {
139+
if (!status.diag_single_point_fault.empty()) {
140+
return HazardStatus::SINGLE_POINT_FAULT;
141+
}
142+
if (!status.diag_latent_fault.empty()) {
143+
return HazardStatus::LATENT_FAULT;
144+
}
145+
if (!status.diag_safe_fault.empty()) {
146+
return HazardStatus::SAFE_FAULT;
147+
}
148+
return HazardStatus::NO_FAULT;
149+
};
150+
151+
HazardStatusStamped hazard = convert_hazard_diagnostics(*msg);
152+
hazard.stamp = msg->stamp;
153+
hazard.status.level = get_system_level(hazard.status);
154+
hazard.status.emergency = hazard.status.level == HazardStatus::SINGLE_POINT_FAULT;
155+
hazard.status.emergency_holding = false;
156+
pub_hazard_->publish(hazard);
157+
}
158+
159+
} // namespace hazard_status_converter
160+
161+
#include <rclcpp_components/register_node_macro.hpp>
162+
RCLCPP_COMPONENTS_REGISTER_NODE(hazard_status_converter::Converter)

‎system/hazard_status_converter/src/converter.hpp

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
// Copyright 2023 The Autoware Contributors
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CONVERTER_HPP_
16+
#define CONVERTER_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <autoware_auto_system_msgs/msg/hazard_status_stamped.hpp>
21+
#include <tier4_system_msgs/msg/diagnostic_graph.hpp>
22+
23+
#include <string>
24+
#include <unordered_map>
25+
#include <vector>
26+
27+
namespace hazard_status_converter
28+
{
29+
30+
class Converter : public rclcpp::Node
31+
{
32+
public:
33+
explicit Converter(const rclcpp::NodeOptions & options);
34+
35+
private:
36+
using DiagnosticGraph = tier4_system_msgs::msg::DiagnosticGraph;
37+
using HazardStatusStamped = autoware_auto_system_msgs::msg::HazardStatusStamped;
38+
rclcpp::Subscription<DiagnosticGraph>::SharedPtr sub_graph_;
39+
rclcpp::Publisher<HazardStatusStamped>::SharedPtr pub_hazard_;
40+
void on_graph(const DiagnosticGraph::ConstSharedPtr msg);
41+
};
42+
43+
} // namespace hazard_status_converter
44+
45+
#endif // CONVERTER_HPP_

0 commit comments

Comments
 (0)