Esempio n. 1
0
    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())
Esempio n. 2
0
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)
Esempio n. 3
0
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())