def test_getters_work(self):
subject = Command(0.1, 0.2)
self.assertEqual(0.1, subject.get_steering())
self.assertEqual(0.2, subject.get_throttle())
subject.values['steering'] = 0.3
subject.values['throttle'] = 0.4
self.assertEqual(0.3, subject.get_steering())
self.assertEqual(0.4, subject.get_throttle())
class VehicleCtl():
"""
Primary vehicle interface class for the client. This class Controls:
- Communication to and from the vehicle
- Autonomous agents
- Video streaming
- Mode control
"""
degradation_factor = 0.1
def __init__(self, config_handler: ConfigHandler) -> None:
super().__init__()
self._config_handler = config_handler
self._time_since_last_throttle_steering_eval = time.time()
self._request_handler = RequestHandler(config_handler)
self._mode = Mode()
self._thread = VehicleCtlThread()
# This represents the last command we sent to the vehicle. It is needed to interpolate
# between the current command and the target command.
self._last_cmd_sent = Command()
# The logical "Gear" we are working with. FIRST, REVERSE, or NEUTRAL
# This is used when we are preparing the command to send to the vehicle
self._gear = Gear.DRIVE
# These represents the intended speed/direction of the vehicle
# VehicleCtlThread uses the target_acceleration to compute a target throttle,
# allowing us to roll onto the throttle, rather than FLOOR IT when the user presses 'forward'.
# Steering, however, is NOT interpolated, and for good reason. If a user steers hard to the left,
# we want the wheels to go to that position as quickly as possible.
self._target_acceleration = 0
self._target_steering = self._last_cmd_sent.get_steering()
# Defines the behavior for our thread loop
self._thread.behave = lambda: self.throttle_steering_eval()
self._stream_port = self._config_handler.get_config_value_or(
'stream_port', 4000)
def throttle_steering_eval(self):
dt = time.time() - self._time_since_last_throttle_steering_eval
prepared_cmd = self.prepare_new_command(dt)
if not prepared_cmd.equal(self._last_cmd_sent):
self._request_handler.send_command(prepared_cmd)
self._last_cmd_sent = prepared_cmd
elif time.time() - self._request_handler.get_time_last_cmd_sent(
) > 0.5:
self._request_handler.send_command(self._last_cmd_sent)
self._time_since_last_throttle_steering_eval = time.time()
time.sleep(0.05)
def prepare_new_command(self, dt):
if self._gear == Gear.REVERSE:
instantaneous_acceleration = min(
(self._target_acceleration + VehicleCtl.degradation_factor),
1.0) * dt
interpolated_throttle = max(
min((self._last_cmd_sent.get_throttle() +
instantaneous_acceleration), 0.0), -1.0)
interpolated_steering = self._target_steering
elif self._gear == Gear.DRIVE:
instantaneous_acceleration = max(
(self._target_acceleration - VehicleCtl.degradation_factor),
-1.0) * dt
interpolated_throttle = min(
max(
self._last_cmd_sent.get_throttle() +
instantaneous_acceleration, 0.0), 1.0)
interpolated_steering = self._target_steering
else:
interpolated_throttle = 0.0
interpolated_steering = 0.0
prepared_cmd = Command(interpolated_steering, interpolated_throttle)
return prepared_cmd
def start(self):
self._thread.start()
def stop(self):
self._thread.stop()
def get_gear(self):
return self._gear
def toggle_fw_bw(self):
if self.get_gear().value == Gear.DRIVE.value:
self._gear = Gear.REVERSE
else:
self._gear = Gear.DRIVE
logging.info("Switched vehicle gear to " + str(self._gear))
def request_stream_stop(self):
self._request_handler.send_image_stream_stop()
def request_stream_start(self):
self._request_handler.send_image_stream_start(self._stream_port)
def restart_stream(self):
self._request_handler.send_image_stream_stop()
self._request_handler.send_image_stream_start(self._stream_port)
def vehicle_ip(self):
return self._request_handler.dest_ip()
def vehicle_port(self):
return self._request_handler.dest_port()
def vehicle_proxy_address(self):
return self._request_handler.proxy_address()
def vehicle_proxy_port(self):
return self._request_handler.proxy_port()
def set_accelerator(self, a_value):
self._target_acceleration = a_value
def set_steering(self, s_value):
self._target_steering = s_value
def mode(self):
return self._mode
def set_mode(self, mode):
logging.info(f"Setting to mode {mode.mode_name()}")
# TODO: Find a place to handle all of the mode switch logic
if self._mode.mode_type(
) == ModeType.TRAIN and mode.mode_type() != ModeType.TRAIN:
# We're moving out of training mode
pass
if self._mode.mode_type() != ModeType.TRAIN and mode.mode_type(
) == ModeType.TRAIN:
# We're moving into training mode
pass
if self._mode.mode_type(
) == ModeType.AUTO and mode.mode_type() != ModeType.AUTO:
# We're moving out of auto mode
pass
if self._mode.mode_type() != ModeType.AUTO and mode.mode_type(
) == ModeType.AUTO:
# Moving into auto
pass
self._mode = mode
def set_proxy(self, address, port):
self._config_handler.set_config_value('proxy_address', address)
self._config_handler.set_config_value('proxy_port', port)
self._request_handler.set_proxy(address, port)
# Need to now restart the image server and image stream
self._image_stream_server.stop()
self._image_stream_server.start()
self.restart_stream()
def is_using_proxy(self):
return self._request_handler.is_using_proxy()
def enable_proxy(self):
self._config_handler.set_config_value('use_proxy', True)
self._request_handler.enable_proxy()
def disable_proxy(self):
self._config_handler.set_config_value('use_proxy', False)
self._request_handler.disable_proxy()
def set_endpoint(self, ip, port):
self._config_handler.set_config_value('vehicle_ip', ip)
self._config_handler.set_config_value('vehicle_port', port)
self._request_handler.set_endpoint(ip, port)
# Need to now restart the image server and image stream
self._image_stream_server.stop()
self._image_stream_server.start()
self.restart_stream()
def get_trim(self):
return self._request_handler.get_trim()
def send_trim(self, trim):
self._request_handler.send_trim(trim)
class CommandThread(threading.Thread):
""" Main thread for the vehicle controller """
def __init__(self, *args, **kwargs):
super(CommandThread, self).__init__(*args, **kwargs)
self._stop_event = threading.Event()
self.command = Command()
self.last_cmd_time = None
self.config_handler = ConfigHandler.get_instance()
# Flag noting whether this is the vehicle or if it's a test server
self.is_vehicle = self.config_handler.get_config_value_or(
'is_vehicle', True)
# Pull in the trim from the config file
self.trim = Trim()
self.trim.from_json(self.config_handler.get_config_value_or(
'trim', {}))
if self.is_vehicle: # initialize throttle and steering values
self.throttle = Throttle(self.trimmed_throttle())
self.steering = Steering(self.trimmed_steering())
self.lock = threading.Lock()
self.loop_delay = 0.01
logging.debug(
"Main thread for vehicle controller started. Awaiting first command..."
)
def stop(self):
self._stop_event.set()
def stopped(self):
return self._stop_event.is_set()
def run(self):
while True:
if self.stopped():
return
with self.lock:
self.execute_command()
time.sleep(self.loop_delay)
def execute_command(self):
if self.last_cmd_time is None:
return
if time.time() - self.last_cmd_time > 1:
logging.debug("Current command expired! Stopping vehicle")
self.command = Command()
self.last_cmd_time = None
if self.is_vehicle:
# Calculate trimmed values and update
trimmed_throttle = self.trimmed_throttle()
trimmed_steering = self.trimmed_steering()
self.throttle.update_throttle(trimmed_throttle)
self.steering.update_steering(trimmed_steering)
def trimmed_throttle(self):
return self.trim.get_trimmed_throttle(self.command.get_throttle())
def trimmed_steering(self):
return self.trim.get_trimmed_steering(self.command.get_steering())
