def __init__(self, socket_file_name):
super(DriverListener, self).__init__()
self.name = self.__class__.__name__
self._socket_file_name = socket_file_name
self._run = True
self._connected = False
self._connection = None
dummy_logger = DummyLogger()
dummy_telemetry = DummyTelemetry(dummy_logger, (100, 100))
self._driver = Driver(dummy_telemetry, dummy_logger)
def main():
"""Main function."""
signal.signal(signal.SIGWINCH, signal_handler)
# First, shut the damn car up
throttle_percentage = 0.0
# And reset the steering
steering_percentage = 0.0
logger = DummyLogger()
telemetry = DummyTelemetry(logger, (40.0182663, -105.2761267))
global driver
driver = Driver(telemetry, logger)
socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
socket_.bind(('', 12345))
socket_.settimeout(2)
try:
data = None
while True:
print(
'Throttle: {}, steering: {}'.format(
throttle_percentage,
steering_percentage
)
)
try:
data, address = socket_.recvfrom(1024) # pylint: disable=unused-variable
command = json.loads(data.decode())
except ValueError as value_error:
print('Unable to parse JSON {}: {}'.format(data, value_error))
continue
except:
print('Timed out')
throttle_percentage = 0.0
steering_percentage = 0.0
command = {}
if 'quit' in command:
break
if 'throttle' in command:
throttle_percentage = float(command['throttle'])
if 'steering' in command:
steering_percentage = float(command['steering'])
driver.drive(throttle_percentage, steering_percentage)
except Exception as exc: # pylint: disable=broad-except
print('Exception: {}'.format(exc))
finally:
driver.drive(0.0, 0.0)
def main():
"""Main function."""
# First, shut the damn car up
throttle_percentage = 0.0
# And reset the steering
steering_percentage = 0.0
logger = DummyLogger()
telemetry = DummyTelemetry(logger, (40.0182663, -105.2761267))
driver = Driver(telemetry, logger)
# driver limits the reverse throttle to 25% to prevent motor damage
driver._get_throttle = (
lambda percentage: int(driver_module.THROTTLE_NEUTRAL_US + driver_module.THROTTLE_DIFF * percentage) // 10 * 10
)
driver.drive(0.0, 0.0)
input(
"""
Disconnect the motor cables. While holding down the setup button on the ESC,
switch on the power. The LED should start changing colors from red -> green ->
orange. Red is for calibrating the throttle high and low points for forward and
reverse. Press setup when the LED is red; the LED will start to single flash
red.
Press enter to continue.
"""
)
driver.drive(1.0, 0.25)
input(
"""
Press the set button. The LED should start to double flash red
Press enter to continue.
"""
)
driver.drive(-1.0, -0.25)
input(
"""
Press the set button. The LED should turn off. That's it!
Press enter to exit.
"""
)
driver.drive(0.0, 0.0)
def main():
"""Main function."""
signal.signal(signal.SIGWINCH, signal_handler)
# First, shut the damn car up
throttle_percentage = 0.0
# And reset the steering
steering_percentage = 0.0
logger = DummyLogger()
telemetry = DummyTelemetry(logger, (40.0182663, -105.2761267))
global driver
driver = Driver(telemetry, logger)
socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
socket_.bind(('', 12345))
socket_.settimeout(2)
try:
data = None
while True:
print('Throttle: {}, steering: {}'.format(throttle_percentage,
steering_percentage))
try:
data, address = socket_.recvfrom(1024) # pylint: disable=unused-variable
command = json.loads(data.decode())
except ValueError as value_error:
print('Unable to parse JSON {}: {}'.format(data, value_error))
continue
except:
print('Timed out')
throttle_percentage = 0.0
steering_percentage = 0.0
command = {}
if 'quit' in command:
break
if 'throttle' in command:
throttle_percentage = float(command['throttle'])
if 'steering' in command:
steering_percentage = float(command['steering'])
driver.drive(throttle_percentage, steering_percentage)
except Exception as exc: # pylint: disable=broad-except
print('Exception: {}'.format(exc))
finally:
driver.drive(0.0, 0.0)
class DriverListener(threading.Thread):
"""Receives commands on a socket from the controlling program to drive."""
def __init__(self, socket_file_name):
super(DriverListener, self).__init__()
self.name = self.__class__.__name__
self._socket_file_name = socket_file_name
self._run = True
self._connected = False
self._connection = None
dummy_logger = DummyLogger()
dummy_telemetry = DummyTelemetry(dummy_logger, (100, 100))
self._driver = Driver(dummy_telemetry, dummy_logger)
def run(self):
"""Runs in a thread. Waits for clients to connects then receives and
handles drive messages.
"""
try:
print('DriverListener waiting for commands')
while self._run:
try:
self.run_socket()
except Exception as exc:
print('Error in DriverListener: {}'.format(exc))
return
except Exception as exc:
print('DriverListener failed with exception {}'.format(exc))
def run_socket(self):
with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
try:
os.unlink(self._socket_file_name)
except Exception:
pass
sock.bind(self._socket_file_name)
pi = pwd.getpwnam('pi')
os.chown(self._socket_file_name, pi.pw_uid, pi.pw_gid)
sock.listen(1)
sock.settimeout(1)
try:
self.wait_for_connections(sock)
except socket.timeout:
return
except socket.error as exc:
print('DriverListener error with socket: {}'.format(exc))
if exc.errno == 32: # Broken pipe
print('DriverListener closing socket')
sock.shutdown(socket.SHUT_RDWR)
sock.close()
return
elif exc.errno == 98: # Address already in use
print('DriverListener quitting waiting for connections')
return
else:
print('Unknown error')
return
def wait_for_connections(self, sock):
while self._run:
self._connection, _ = sock.accept()
self._connected = True
print('DriverListener client connected')
# Now we're connected, so just wait until someone
# calls handle_message
while self._run:
try:
command = self._connection.recv(4096)
throttle, steering = [float(i) for i in command.split(' ')]
print('DriverListener driving {} {}'.format(throttle, steering))
self._driver.drive(throttle, steering)
except socket.timeout:
continue
def kill(self):
"""Stops the thread."""
self._run = False
def main():
"""Main function."""
# First, shut the damn car up
throttle_percentage = 0.0
# And reset the steering
steering_percentage = 0.0
logger = DummyLogger()
telemetry = DummyTelemetry(logger, (40.0182663, -105.2761267))
driver = Driver(telemetry, logger)
# driver limits the reverse throttle to 25% to prevent motor damage
driver._get_throttle = lambda percentage: \
int(
driver_module.THROTTLE_NEUTRAL_US
+ driver_module.THROTTLE_DIFF
* percentage
) // 10 * 10
driver.drive(0.0, 0.0)
input('''
Disconnect the motor cables. While holding down the setup button on the ESC,
switch on the power. The LED should start changing colors from red -> green ->
orange. Red is for calibrating the throttle high and low points for forward and
reverse. Press setup when the LED is red; the LED will start to single flash
red.
Press enter to continue.
''')
driver.drive(1.0, 0.25)
input('''
Press the set button. The LED should start to double flash red
Press enter to continue.
''')
driver.drive(-1.0, -0.25)
input('''
Press the set button. The LED should turn off. That's it!
Press enter to exit.
''')
driver.drive(0.0, 0.0)
def start_threads(
waypoint_generator,
logger,
web_socket_handler,
max_throttle,
kml_file_name,
):
"""Runs everything."""
logger.info('Creating Telemetry')
telemetry = Telemetry(kml_file_name)
telemetry_dumper = TelemetryDumper(
telemetry,
waypoint_generator,
web_socket_handler
)
logger.info('Done creating Telemetry')
global DRIVER
DRIVER = Driver(telemetry)
DRIVER.set_max_throttle(max_throttle)
logger.info('Setting SUP800F to NMEA mode')
serial_ = serial.Serial('/dev/ttyAMA0', 115200)
serial_.setTimeout(1.0)
for _ in range(10):
serial_.readline()
try:
switch_to_nmea_mode(serial_)
except: # pylint: disable=W0702
logger.error('Unable to set mode')
for _ in range(10):
serial_.readline()
logger.info('Done')
# The following objects must be created in order, because of message
# exchange dependencies:
# sup800f_telemetry: reads from command forwarded
# command: reads from command, writes to command forwarded
# button: writes to command
# cherry_py_server: writes to command
# TODO(2016-08-21) Have something better than sleeps to work around race
# conditions
logger.info('Creating threads')
sup800f_telemetry = Sup800fTelemetry(serial_)
time.sleep(0.5)
command = Command(telemetry, DRIVER, waypoint_generator)
time.sleep(0.5)
button = Button()
port = int(get_configuration('PORT', 8080))
address = get_configuration('ADDRESS', '0.0.0.0')
cherry_py_server = CherryPyServer(
port,
address,
telemetry,
waypoint_generator
)
time.sleep(0.5)
global THREADS
THREADS += (
button,
cherry_py_server,
command,
sup800f_telemetry,
telemetry_dumper,
)
for thread in THREADS:
thread.start()
logger.info('Started all threads')
# Use a fake timeout so that the main thread can still receive signals
sup800f_telemetry.join(100000000000)
# Once we get here, sup800f_telemetry has died and there's no point in
# continuing because we're not receiving telemetry messages any more, so
# stop the command module
command.stop()
command.join(100000000000)
cherry_py_server.kill()
cherry_py_server.join(100000000000)
button.kill()
button.join(100000000000)
