diff --git a/src/signage/src/signage/autoware_interface.py b/src/signage/src/signage/autoware_interface.py index 2b268e7..e1cb3e8 100644 --- a/src/signage/src/signage/autoware_interface.py +++ b/src/signage/src/signage/autoware_interface.py @@ -98,7 +98,7 @@ def reset_timer(self): if utils.check_timeout( self._node.get_clock().now(), self._autoware_connection_time, DISCONNECT_THRESHOLD ): - self.information = AutowareInformation() + self.information.mrm_behavior = MrmState.NONE self._node.get_logger().error( "Autoware disconnected", throttle_duration_sec=DISCONNECT_THRESHOLD ) diff --git a/src/signage/src/signage/route_handler.py b/src/signage/src/signage/route_handler.py index 15f894d..3b84dd9 100644 --- a/src/signage/src/signage/route_handler.py +++ b/src/signage/src/signage/route_handler.py @@ -91,8 +91,15 @@ def emergency_checker_callback(self): 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 + self._in_slowing_state = ( + self._autoware.information.motion_state == MotionState.MOVING + ) + self._in_slow_stop_state = ( + self._autoware.information.motion_state == MotionState.STOPPED + ) + else: + self._in_slowing_state = False + self._in_slow_stop_state = False if in_emergency and not self._in_emergency_state: self._announce_interface.announce_emergency("emergency") @@ -298,7 +305,7 @@ def route_checker_callback(self): self._service_interface.trigger_external_signage(True) self._trigger_external_signage = True self._announce_engage = True - elif self._autoware.information.route_state == RouteState.ARRIVED: + elif self._autoware.information.route_state in [RouteState.ARRIVED, RouteState.UNSET]: # Check whether the vehicle arrive to goal self._is_driving = False self._is_stopping = True