def set_mode(self, mode):
if mode not in [
SimManager.MODE_DISABLED, SimManager.MODE_AUTONOMOUS,
SimManager.MODE_OPERATOR_CONTROL, SimManager.MODE_TEST
]:
raise ValueError("Invalid value for mode: %s" % mode)
with self._lock:
# TODO: need a way to notify the caller that the set failed. Perhaps an exception?
if not self.is_alive():
return
old_mode = self.mode
self.mode = mode
callback = self.mode_callback
# don't call from inside the lock
if old_mode != mode:
if mode == SimManager.MODE_DISABLED:
mode_helpers.set_disabled()
elif mode == SimManager.MODE_AUTONOMOUS:
mode_helpers.set_autonomous(True)
elif mode == SimManager.MODE_OPERATOR_CONTROL:
mode_helpers.set_teleop_mode(True)
elif mode == SimManager.MODE_TEST:
mode_helpers.set_test_mode(True)
self.physics_controller._set_robot_enabled(
mode != SimManager.MODE_DISABLED)
if callback is not None:
callback(mode)
def set_mode(self, mode):
if mode not in [SimManager.MODE_DISABLED,
SimManager.MODE_AUTONOMOUS,
SimManager.MODE_OPERATOR_CONTROL,
SimManager.MODE_TEST]:
raise ValueError("Invalid value for mode: %s" % mode)
with self._lock:
# TODO: need a way to notify the caller that the set failed. Perhaps an exception?
if not self.is_alive():
return
old_mode = self.mode
self.mode = mode
callback = self.mode_callback
# don't call from inside the lock
if old_mode != mode:
if mode == SimManager.MODE_DISABLED:
mode_helpers.set_disabled()
elif mode == SimManager.MODE_AUTONOMOUS:
mode_helpers.set_autonomous(True)
elif mode == SimManager.MODE_OPERATOR_CONTROL:
mode_helpers.set_teleop_mode(True)
elif mode == SimManager.MODE_TEST:
mode_helpers.set_test_mode(True)
self.physics_controller._set_robot_enabled(mode != SimManager.MODE_DISABLED)
if callback is not None:
callback(mode)
def on_state(self, msg: DriverStation):
logger.info("on state: %s", msg)
if self.state != msg.state or self.enabled != msg.enabled:
if msg.state == DriverStation.TEST:
mode_helpers.set_test_mode(msg.enabled)
elif msg.state == DriverStation.AUTO:
mode_helpers.set_autonomous(msg.enabled)
elif msg.state == DriverStation.TELEOP:
mode_helpers.set_teleop_mode(msg.enabled)
self.state = msg.state
self.enabled = msg.enabled
def on_state(self, msg):
state = DriverStation.FromString(msg)
if self.state != state.state or self.enabled != state.enabled:
if state.state == DriverStation.TEST:
mode_helpers.set_test_mode(state.enabled)
elif state.state == DriverStation.AUTO:
mode_helpers.set_autonomous(state.enabled)
elif state.state == DriverStation.TELEOP:
mode_helpers.set_teleop_mode(state.enabled)
self.state = state.state
self.enabled = state.enabled
def on_state(self, msg):
state = DriverStation.FromString(msg)
if self.state != state.state or self.enabled != state.enabled:
if state.state == DriverStation.TEST:
mode_helpers.set_test_mode(state.enabled)
elif state.state == DriverStation.AUTO:
mode_helpers.set_autonomous(state.enabled)
elif state.state == DriverStation.TELEOP:
mode_helpers.set_teleop_mode(state.enabled)
self.state = state.state
self.enabled = state.enabled