119 lines
3.7 KiB
Python
Executable File
119 lines
3.7 KiB
Python
Executable File
#!/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()
|