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 <[email protected]> Co-authored-by: Kah Hooi Tan <[email protected]>
tier4 · Aug 20, 2024 · 286e292 · 286e292
1 parent 2a6cf7d
commit 286e292
Showing 1 changed file with 19 additions and 15 deletions.
diff --git 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