def main():
configurations.parseArgs(sys.argv)
listener = Listener()
if configurations.ARGS["quiet"]:
print("Running in quiet mode")
while True:
try:
print("What's up?")
if not configurations.ARGS["quiet"]:
command = Command(listener.listen())
else:
command = Command(input())
if command is not None and command.name is not None:
print("Command " + command.name)
if command.command_type == CommandTypes.AUDIO:
# Music procssor here
audioProcessor = AudioProcessor()
audioProcessor.process(command)
elif command.command_type == CommandTypes.SYSTEM:
#System processor here
systemProcessor = SystemProcessor()
systemProcessor.process(command)
elif command.command_type == CommandTypes.SOCIAL:
mediaProcessor = SocialMediaProcessor()
mediaProcessor.process(command)
elif command.name.lower() == "exit":
print("Exiting")
sys.exit(0)
else:
print("Command not recognized.")
else:
print("I'm sorry, I didn't catch that")
except CancelCommand:
pass
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 __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 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 test_should_enforce_lower_boundary_on_throttle(self):
subject = Command()
# test just above the upper bound to make sure it does throw an exception
self.assertRaises(Exception, lambda: subject.set_throttle(-1.1))
# test the upper bound to make sure it does not throw an exception
subject.set_throttle(-1.0)
def get_command(self):
with self.mic as source:
self.recognizer.adjust_for_ambient_noise(source)
audio = self.recognizer.listen(source)
try:
return Command(self.recognizer.recognize(audio))
except LookupError:
return None
def test_prepare_new_command_reverse_stationary(self):
subject = VehicleCtl(self.mock_config_handler)
subject._gear = Gear.REVERSE
subject._target_steering = 0.0
subject._target_acceleration = 0.0
subject._last_cmd_sent = Command(0.0, 0.0)
new_cmd = subject.prepare_new_command(self.dt)
self.assertEqual(new_cmd.get_steering(), 0.0)
self.assertEqual(new_cmd.get_throttle(), 0.0)
def test_prepare_new_command_park_outputs_stationary_command(self):
subject = VehicleCtl(self.mock_config_handler)
subject._gear = Gear.PARK
subject._target_steering = 0.23
subject._target_acceleration = -0.34
subject._last_cmd_sent = Command(0.22, -1.0)
new_cmd = subject.prepare_new_command(self.dt)
self.assertEqual(new_cmd.get_steering(), 0.0)
self.assertEqual(new_cmd.get_throttle(), 0.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())
def test_prepare_new_command_forward_max_at_1_point_0(self):
subject = VehicleCtl(self.mock_config_handler)
subject._gear = Gear.DRIVE
subject._target_steering = 0.0
subject._target_acceleration = 1.0
subject._last_cmd_sent = Command(s=0.0, t=1.0)
new_cmd = subject.prepare_new_command(self.dt)
self.assertEqual(new_cmd.get_steering(), 0.0)
self.assertEqual(new_cmd.get_throttle(), 1.0)
def test_should_init_with_expected_values(self):
subject = VehicleCtl(self.mock_config_handler)
# training manager, auto agent, etc...
self.assertIsInstance(subject._mode, Mode)
# Vehicle throttle,steering, etc...
self.assertTrue(subject._last_cmd_sent.equal(Command()))
self.assertEqual(Gear.DRIVE, subject._gear)
self.assertEqual(0, subject._target_acceleration)
self.assertEqual(subject._last_cmd_sent.get_steering(),
subject._target_steering)
def test_prepare_new_command_forward_coasting(self):
subject = VehicleCtl(self.mock_config_handler)
subject._gear = Gear.DRIVE
subject._target_steering = 0.0
subject._target_acceleration = 0.0
subject._last_cmd_sent = Command(s=0.0, t=1.0)
expected_instant_acc = (0.0 - VehicleCtl.degradation_factor) * self.dt
new_cmd = subject.prepare_new_command(self.dt)
self.assertEqual(new_cmd.get_steering(), 0.0)
self.assertEqual(new_cmd.get_throttle(), 1.0 + expected_instant_acc)
def test_prepare_new_command_forward_braking(self):
subject = VehicleCtl(self.mock_config_handler)
subject._gear = Gear.DRIVE
subject._target_steering = 0.0
subject._target_acceleration = -1.0
subject._last_cmd_sent = Command(s=0.0, t=1.0)
expected_instant_acc = -1.0 * self.dt #this test verifies that instant_acc is not less than -1, before considering dt
new_cmd = subject.prepare_new_command(self.dt)
self.assertEqual(new_cmd.get_steering(), 0.0)
self.assertEqual(new_cmd.get_throttle(), 1.0 + expected_instant_acc)
def test_prepare_new_command_reverse_coasting(self):
subject = VehicleCtl(self.mock_config_handler)
subject._gear = Gear.REVERSE
subject._target_steering = 0.0
subject._target_acceleration = 0.0
subject._last_cmd_sent = Command(0.0, -1.0)
expectedAcc = (0.0 + VehicleCtl.degradation_factor) * self.dt
new_cmd = subject.prepare_new_command(self.dt)
self.assertEqual(new_cmd.get_steering(), 0.0)
self.assertEqual(new_cmd.get_throttle(), -1.0 + expectedAcc)
def test_setters_work(self):
subject = Command(0.1, 0.2)
subject.set_steering(0.3)
subject.set_throttle(0.4)
self.assertEqual(0.3, subject.values['steering'])
self.assertEqual(0.4, subject.values['throttle'])
subject.set_steering(0.5)
subject.set_throttle(0.6)
self.assertEqual(0.5, subject.values['steering'])
self.assertEqual(0.6, subject.values['throttle'])
def handle_message(serv, usr, data): try: msg = Command(data) (state, handler) = handler_map[msg.command] if check_state(serv, usr, state): handler(serv, usr, *msg.arguments) except ValueError: pass except KeyError: unknown_handler(serv, usr, msg.command) except TypeError: invalid_handler(serv, usr, msg.command)
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 call(self, command, *args):
"""
Synchronously call and get reply
"""
cmd = Command(command, *args)
# send command
with self._command_lock:
self._command_store[cmd.id] = cmd
self._network.send(str(cmd) + self._split)
# await reply
reply = cmd.wait()
# delete command
with self._command_lock:
del self._command_store[cmd.id]
return reply
def test_should_check_steering_bounds_on_init(self):
self.assertRaises(Exception, lambda: Command(s=1.1))
def test_should_init_with_default_values(self):
subject = Command()
self.assertEqual(0.0, subject.values['steering'])
self.assertEqual(0.0, subject.values['throttle'])
def broadcast(self, command, *args):
""" Send a message to all connected clients """
cmd = Command(command, *args, id='BROADCAST')
for client in self.clients():
client.write(str(cmd) + self._split)
def unicast(self, connection, command, *args):
""" Send a message to a specific client """
cmd = Command(command, *args, id='UNICAST')
connection.write(str(cmd) + self._split)
def test_should_init_with_supplied_values(self):
subject = Command(0.1, 0.2)
self.assertEqual(0.1, subject.values['steering'])
self.assertEqual(0.2, subject.values['throttle'])
def test_should_check_throttle_bounds_on_init(self):
self.assertRaises(Exception, lambda: Command(t=-1.1))
