From 286e29273159d14b4a2d680d6c91aa076ae366ac Mon Sep 17 00:00:00 2001 From: yabuta Date: Tue, 20 Aug 2024 19:09:24 +0900 Subject: [PATCH] fix ignore emergency parameter not working (#110) (#112) * fix ignore emergency parameter not working * add comfortable stop condition * change comfortable stop condition --------- Signed-off-by: tkhmy Co-authored-by: Kah Hooi Tan <41041286+tkhmy@users.noreply.github.com> --- src/signage/src/signage/route_handler.py | 34 +++++++++++++----------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py index 1baab28..57bd292 100644 --- a/src/signage/src/signage/route_handler.py +++ b/src/signage/src/signage/route_handler.py @@ -75,22 +75,22 @@ def __init__( self._node.create_timer(0.2, self.announce_engage_when_starting) def emergency_checker_callback(self): - if self._parameter.ignore_emergency: - in_emergency = False - if self._autoware.information.operation_mode == OperationModeState.STOP: + if ( + self._parameter.ignore_emergency + or self._autoware.information.operation_mode == OperationModeState.STOP + ): in_emergency = False + self._in_slowing_state = False + self._in_slow_stop_state = False else: in_emergency = self._autoware.information.mrm_behavior == MrmState.EMERGENCY_STOP - self._in_slowing_state = ( - self._autoware.information.mrm_behavior == MrmState.COMFORTABLE_STOP - 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 - or self._autoware.information.mrm_behavior == MrmState.PULL_OVER - ) and self._autoware.information.motion_state == MotionState.STOPPED + if self._autoware.information.mrm_behavior in [ + MrmState.COMFORTABLE_STOP, + MrmState.PULL_OVER, + ]: + self._in_slowing_state == self._autoware.information.motion_state == MotionState.MOVING + self._in_slow_stop_state == self._autoware.information.motion_state == MotionState.STOPPED if in_emergency and not self._in_emergency_state: self._announce_interface.announce_emergency("emergency") @@ -127,8 +127,8 @@ def announce_engage_when_starting(self): if ( self._autoware.information.localization_init_state - == LocalizationInitializationState.UNINITIALIZED or - self._autoware.information.autoware_control == False + == LocalizationInitializationState.UNINITIALIZED + or self._autoware.information.autoware_control == False ): self._prev_motion_state = 0 return @@ -286,7 +286,11 @@ def route_checker_callback(self): ): self._service_interface.trigger_external_signage(True) self._trigger_external_signage = True - if not self._announce_engage and self._parameter.signage_stand_alone and self._autoware.information.autoware_control: + if ( + not self._announce_engage + and self._parameter.signage_stand_alone + and self._autoware.information.autoware_control + ): self._announce_interface.send_announce("engage") self._service_interface.trigger_external_signage(True) self._trigger_external_signage = True