Initial import of FaRui Campus ADS v3.2

2026-06-05 14:20:30 +08:00
commit 2839d34fdb
6548 changed files with 1335203 additions and 0 deletions
@@ -0,0 +1,118 @@
+#!/usr/bin/env python3
+
+from collections import deque
+
+import rclpy
+from autoware_adapi_v1_msgs.msg import OperationModeState
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+
+
+class OperationModeStateFallback(Node):
+    def __init__(self):
+        super().__init__("planning_debug_operation_mode_fallback")
+
+        self.topic = (
+            self.declare_parameter("topic", "/system/operation_mode/state")
+            .get_parameter_value()
+            .string_value
+        )
+        self.period_sec = (
+            self.declare_parameter("period_sec", 0.5).get_parameter_value().double_value
+        )
+        self.timeout_sec = (
+            self.declare_parameter("timeout_sec", 1.5).get_parameter_value().double_value
+        )
+
+        qos = QoSProfile(depth=1)
+        qos.reliability = ReliabilityPolicy.RELIABLE
+        qos.durability = DurabilityPolicy.TRANSIENT_LOCAL
+
+        self.pub = self.create_publisher(OperationModeState, self.topic, qos)
+
+        sub_qos = QoSProfile(depth=1)
+        sub_qos.reliability = ReliabilityPolicy.RELIABLE
+        sub_qos.durability = DurabilityPolicy.VOLATILE
+        self.sub = self.create_subscription(
+            OperationModeState, self.topic, self.on_state, sub_qos
+        )
+
+        self.timer = self.create_timer(self.period_sec, self.on_timer)
+        self.publishing_fallback = False
+        self.last_msg_time = None
+        self.own_stamps = deque(maxlen=32)
+
+        self.get_logger().info(
+            f"Monitoring {self.topic}; publishing STOP fallback when no fresh message exists."
+        )
+
+    @staticmethod
+    def stamp_key(stamp):
+        return stamp.sec, stamp.nanosec
+
+    def on_state(self, msg):
+        if self.stamp_key(msg.stamp) in self.own_stamps:
+            return
+        self.last_msg_time = self.get_clock().now()
+
+    def has_external_publisher(self):
+        own_name = self.get_name()
+        own_namespace = self.get_namespace()
+
+        for info in self.get_publishers_info_by_topic(self.topic):
+            if info.node_name != own_name or info.node_namespace != own_namespace:
+                return True
+        return False
+
+    def has_fresh_message(self):
+        if self.last_msg_time is None:
+            return False
+
+        age_sec = (self.get_clock().now() - self.last_msg_time).nanoseconds * 1e-9
+        return age_sec <= self.timeout_sec
+
+    def on_timer(self):
+        if self.has_fresh_message():
+            if self.publishing_fallback:
+                self.get_logger().info(
+                    f"Detected fresh {self.topic}; fallback is idle."
+                )
+                self.publishing_fallback = False
+            return
+
+        msg = OperationModeState()
+        msg.stamp = self.get_clock().now().to_msg()
+        msg.mode = OperationModeState.STOP
+        msg.is_autoware_control_enabled = False
+        msg.is_in_transition = False
+        msg.is_stop_mode_available = True
+        msg.is_autonomous_mode_available = False
+        msg.is_local_mode_available = False
+        msg.is_remote_mode_available = False
+        self.own_stamps.append(self.stamp_key(msg.stamp))
+        self.pub.publish(msg)
+
+        if not self.publishing_fallback:
+            reason = (
+                "no external publisher"
+                if not self.has_external_publisher()
+                else f"no fresh message within {self.timeout_sec:.1f}s"
+            )
+            self.get_logger().warn(
+                f"{reason} on {self.topic}; publishing STOP fallback for planning."
+            )
+            self.publishing_fallback = True
+
+
+def main():
+    rclpy.init()
+    node = OperationModeStateFallback()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()