diff --git a/src/signage/src/signage/autoware_interface.py b/src/signage/src/signage/autoware_interface.py index 6fd9479..2b268e7 100644 --- a/src/signage/src/signage/autoware_interface.py +++ b/src/signage/src/signage/autoware_interface.py @@ -17,6 +17,7 @@ DISCONNECT_THRESHOLD = 2 + @dataclass class AutowareInformation: autoware_control: bool = False @@ -94,9 +95,13 @@ def __init__(self, node): self._node.create_timer(1, self.reset_timer) def reset_timer(self): - if utils.check_timeout(self._node.get_clock().now(), self._autoware_connection_time, DISCONNECT_THRESHOLD): + if utils.check_timeout( + self._node.get_clock().now(), self._autoware_connection_time, DISCONNECT_THRESHOLD + ): self.information = AutowareInformation() - self._node.get_logger().error("Autoware disconnected", throttle_duration_sec=DISCONNECT_THRESHOLD) + self._node.get_logger().error( + "Autoware disconnected", throttle_duration_sec=DISCONNECT_THRESHOLD + ) self.is_disconnected = True else: self.is_disconnected = False diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py index f1e29ee..0fdfc62 100644 --- a/src/signage/src/signage/route_handler.py +++ b/src/signage/src/signage/route_handler.py @@ -82,13 +82,13 @@ def emergency_checker_callback(self): self._in_slowing_state = ( self._autoware.information.mrm_behavior == MrmState.COMFORTABLE_STOP - and self._autoware.information.motion_state == MotionState.MOVING - ) + or self._autoware.information.mrm_behavior == MrmState.PULL_OVER + ) and self._autoware.information.motion_state == MotionState.MOVING self._in_slow_stop_state = ( self._autoware.information.mrm_behavior == MrmState.COMFORTABLE_STOP - and self._autoware.information.motion_state == MotionState.STOPPED - ) + or self._autoware.information.mrm_behavior == MrmState.PULL_OVER + ) and self._autoware.information.motion_state == MotionState.STOPPED if in_emergency and not self._in_emergency_state: self._announce_interface.announce_emergency("emergency")