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
def on_message(self, message):
'''Called on incoming messages from the simulation. Incoming messages
are JSON encoded, with a 'msgtype' field. The rest of the JSON depends
on what 'msgtype' is.
Current values for msgtype:
* input: Then 'data' is filled with data to be updated in HAL
* mode: then 'mode' and 'enabled' are set
'''
msg = json.loads(message)
msgtype = msg['msgtype']
if msgtype == 'input':
update_hal_data(msg['data'])
elif msgtype == 'add_device_gyro_channel':
self.add_device_gyro_channel(msg['angle_key'])
elif msgtype == 'update_gyros':
self.update_gyros(msg['da'])
elif msgtype == 'mode':
if self.is_mode_the_same(msg['mode'], msg['enabled']) is False:
fake_time.mode_start_tm = fake_time.get()
mode_helpers.set_mode(msg['mode'], msg['enabled'])
elif msgtype == 'set_autonomous':
if self.is_mode_the_same('auto', True) is False:
fake_time.mode_start_tm = fake_time.get()
mode_helpers.set_autonomous(True, msg['game_specific_message'])
elif msgtype == 'pause_sim':
fake_time.pause()
elif msgtype == 'resume_sim':
fake_time.resume()
elif msgtype == 'step_time':
try:
tm = float(msg['time'])
if tm > 0:
fake_time.resume(tm)
except ValueError:
logger.error("Invalid step time", "'%s' is not a valid number", msg['time'])
