Skip to content

Commit 3d45a44

Browse files
TomohitoAndoTomohitoAndo
committed
wip
Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
1 parent 99f9713 commit 3d45a44

File tree

2 files changed

+131
-3
lines changed

2 files changed

+131
-3
lines changed

‎launch/tier4_system_launch/launch/system.launch.xml

+17-3
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@
22
<launch>
33
<!-- Parameter files -->
44
<arg name="component_state_monitor_topic_path"/>
5+
<arg name="diagnostic_graph_aggregator_param_path"/>
6+
<arg name="diagnostic_graph_aggregator_graph_path"/>
57
<arg name="emergency_handler_param_path"/>
68
<arg name="duplicated_node_checker_param_path"/>
79
<arg name="mrm_comfortable_stop_operator_param_path"/>
@@ -61,8 +63,20 @@
6163
</include>
6264
</group>
6365

64-
<!-- Error Monitor -->
66+
<!-- Diagnostic Graph Aggregator -->
67+
<node pkg="diagnostic_graph_aggregator" exec="aggregator" name="diagnostic_graph_aggregator_self">
68+
<param from="$(var diagnostic_graph_aggregator_param_path)"/>
69+
<param name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)"/>
70+
</node>
71+
72+
<!-- temporary for testing -->
6573
<group>
74+
<push-ros-namespace namespace="/mrm"/>
75+
<include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
76+
</group>
77+
78+
<!-- Error Monitor -->
79+
<!-- <group>
6680
<let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
6781
<let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
6882
<let name="config_file" value="$(var system_error_monitor_planning_simulator_param_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
@@ -73,8 +87,8 @@
7387
<arg name="extra_agg_config_file_vehicle" value="$(var diagnostic_aggregator_vehicle_param_path)"/>
7488
<arg name="use_emergency_hold" value="false"/>
7589
</include>
76-
</group>
77-
90+
</group> -->
91+
7892
<!-- Emergency Handler -->
7993
<group>
8094
<include file="$(find-pkg-share emergency_handler)/launch/emergency_handler.launch.xml">

‎system/diagnostic_graph_aggregator/example/test_diags.py

+114
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
#!/usr/bin/env python3
2+
3+
# Copyright 2023 The Autoware Contributors
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
17+
from diagnostic_msgs.msg import DiagnosticArray
18+
from diagnostic_msgs.msg import DiagnosticStatus
19+
import rclpy
20+
import rclpy.node
21+
import rclpy.qos
22+
23+
24+
class DummyDiagnostics(rclpy.node.Node):
25+
def __init__(self, names):
26+
super().__init__("dummy_diagnostics")
27+
qos = rclpy.qos.qos_profile_system_default
28+
self.diags = self.create_publisher(DiagnosticArray, "/diagnostics", qos)
29+
# Timer set to trigger `on_timer` method every 0.5 seconds
30+
self.timer = self.create_timer(0.5, self.on_timer)
31+
self.count = 0
32+
# Create a status for each name provided
33+
self.array = [self.create_status(name) for name in names]
34+
35+
def on_timer(self):
36+
# Calculate the current phase in the loop based on `self.count`
37+
# Each phase lasts 10 counts (5 seconds), cycling through the specified states
38+
transition_size = 7
39+
phase = (self.count // 5) % transition_size
40+
print(phase)
41+
42+
if phase == 0:
43+
print("All OK")
44+
45+
# Loop through each diagnostic status to set its level and message
46+
for status in self.array:
47+
# Default to OK status
48+
status.level = DiagnosticStatus.OK
49+
status.message = "OK"
50+
51+
# Identify the specific condition based on the current phase
52+
if phase == 1 and "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature" in status.name:
53+
print("pandar_temperature WARN")
54+
status.level = DiagnosticStatus.WARN
55+
status.message = "WARN"
56+
elif phase == 2 and "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature" in status.name:
57+
print("pandar_temperature ERROR")
58+
status.level = DiagnosticStatus.ERROR
59+
status.message = "ERROR"
60+
elif phase == 3 and "pandar_monitor: /sensing/lidar/front_lower: pandar_connection" in status.name:
61+
print("pandar_connection WARN")
62+
status.level = DiagnosticStatus.WARN
63+
status.message = "WARN"
64+
elif phase == 4 and "pandar_monitor: /sensing/lidar/front_lower: pandar_connection" in status.name:
65+
print("pandar_connection ERROR")
66+
status.level = DiagnosticStatus.ERROR
67+
status.message = "ERROR"
68+
elif phase == 5 and "imu_monitor: yaw_rate_status" in status.name:
69+
print("yaw_rate WARN")
70+
status.level = DiagnosticStatus.WARN
71+
status.message = "WARN"
72+
elif phase == 6 and "imu_monitor: yaw_rate_status" in status.name:
73+
print("yaw_rate ERROR")
74+
status.level = DiagnosticStatus.ERROR
75+
status.message = "ERROR"
76+
77+
# Publish the diagnostic array with updated statuses
78+
diagnostics = DiagnosticArray()
79+
diagnostics.header.stamp = self.get_clock().now().to_msg()
80+
diagnostics.status = self.array
81+
self.diags.publish(diagnostics)
82+
83+
# Increment the count, effectively moving time forward by 0.5 seconds
84+
self.count = (self.count + 1) % 10000
85+
print(self.count)
86+
87+
@staticmethod
88+
def create_status(name: str):
89+
# Create and return a DiagnosticStatus with a given name and a generic hardware_id
90+
return DiagnosticStatus(name=name, hardware_id="example")
91+
92+
93+
if __name__ == "__main__":
94+
95+
positions = [
96+
'front_lower', 'front_upper',
97+
'left_lower', 'left_upper',
98+
'right_lower', 'right_upper',
99+
'rear_lower', 'rear_upper'
100+
]
101+
types = ['pandar_temperature', 'pandar_connection']
102+
103+
print("### diags ###")
104+
diags = [
105+
f"pandar_monitor: /sensing/lidar/{position}: {type_}"
106+
for position in positions for type_ in types
107+
]
108+
diags += ["imu_monitor: yaw_rate_status"]
109+
110+
print(diags)
111+
112+
rclpy.init()
113+
rclpy.spin(DummyDiagnostics(diags))
114+
rclpy.shutdown()

0 commit comments

Comments
 (0)