|
| 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