diff --git a/autonav_ws/src/autonav_serial/src/serial_node.py b/autonav_ws/src/autonav_serial/src/serial_node.py index bcf3433..e9e1714 100644 --- a/autonav_ws/src/autonav_serial/src/serial_node.py +++ b/autonav_ws/src/autonav_serial/src/serial_node.py @@ -83,6 +83,13 @@ def system_state_transition(self, old: SystemStateEnum, updated: SystemStateEnum except can.CanError: pass + if old.state == SystemStateEnum.AUTONOMOUS and updated.state != SystemStateEnum.AUTONOMOUS: + can_msg = can.Message(arbitration_id=MOBILITY_STOP_ID, data=bytes([0])) + try: + self.can.send(can_msg) + except can.CanError: + pass + if old.state == SystemStateEnum.MANUAL and updated.state == SystemStateEnum.AUTONOMOUS: self.zero_motors()