diff --git a/.gitignore b/.gitignore index 989a5d5..1599b8d 100644 --- a/.gitignore +++ b/.gitignore @@ -16,4 +16,5 @@ **_pb2* **.egg-info* **/dist/* -build \ No newline at end of file +build +__pycache__ \ No newline at end of file diff --git a/car/src/car/control/gpio/factory.py b/car/src/car/control/gpio/factory.py new file mode 100644 index 0000000..69df0a1 --- /dev/null +++ b/car/src/car/control/gpio/factory.py @@ -0,0 +1,22 @@ +from .mockvehicle import MockVehicle +import os + + +def get_vehicle(motor_pin=19, steering_pin=18): + ENV_CAR = None + try: + ENV_CAR = os.environ['CAR_VEHICLE'] + except KeyError: + print('Failed to find CAR_VEHICLE environment variable. Using mock.') + return MockVehicle() + if ENV_CAR == "CAR_2D": + try: + from .vehicle import Vehicle + return Vehicle(motor_pin, steering_pin) + except ImportError: + print( + 'Could not import CAR_2D vehicle. Have you installed the GPIOZERO package?') + elif ENV_CAR == "CAR_MOCK": + return MockVehicle(motor_pin, steering_pin) + else: + print('No valid vehicle found. Have you set the CAR_VEHICLE environment variable?') diff --git a/car/src/car/control/gpio/mockvehicle.py b/car/src/car/control/gpio/mockvehicle.py index 9b0c8ec..b5a08d3 100644 --- a/car/src/car/control/gpio/mockvehicle.py +++ b/car/src/car/control/gpio/mockvehicle.py @@ -5,6 +5,7 @@ class MockVehicle: def __init__(self, motor_pin=19, servo_pin=18): self.motor_pin = motor_pin self.steering_pin = servo_pin + print('Using Mock Vehicle') @property def throttle(self): diff --git a/car/src/car/control/gpio/vehicle.py b/car/src/car/control/gpio/vehicle.py index 9ae941b..f2f0deb 100644 --- a/car/src/car/control/gpio/vehicle.py +++ b/car/src/car/control/gpio/vehicle.py @@ -14,6 +14,7 @@ def _safely_set_servo_value(servo, value): return False return True + def _is_pin_valid(pin): if isinstance(pin, int): if pin < 2 or pin > 21: @@ -26,6 +27,8 @@ def _is_pin_valid(pin): # TODO: Allow a vector to be set to change the throttle/steering, for vehicles that don't use # two servos for controls (e.g. drone, dog) + + class Vehicle: def __init__(self, motor_pin=19, servo_pin=18): subprocess.call(['sudo', 'pigpiod']) @@ -39,7 +42,8 @@ class Vehicle: def initialise_motor(self): self._motor_servo = Servo( self._motor_pin, pin_factory=Device.pin_factory) - self._steering_servo = Servo(self._steering_pin, pin_factory=Device.pin_factory) + self._steering_servo = Servo( + self._steering_pin, pin_factory=Device.pin_factory) @property def throttle(self): @@ -73,11 +77,13 @@ class Vehicle: @steering_pin.setter def steering_pin(self, value): - self._steering_pin = value if _is_pin_valid(value) else self._steering_pin + self._steering_pin = value if _is_pin_valid( + value) else self._steering_pin def stop(self): self.throttle = 0 self.steering = 0 def change_with_vector(self, vector): + # TBD how this will work. Could just be (magnitude,angle) change pass diff --git a/car/src/car/controller.py b/car/src/car/controller.py index de63332..36aecc2 100755 --- a/car/src/car/controller.py +++ b/car/src/car/controller.py @@ -7,7 +7,7 @@ import time import grpc import car.control.motorService_pb2_grpc as motorService_pb2_grpc -from car.control.gpio.vehicle import Vehicle +import car.control.gpio.factory as vehicle_factory from car.control.motor_servicer import MotorServicer from car.slam.slam_servicer import SlamServicer import car.slam.SlamController_pb2_grpc as SlamController_pb2_grpc @@ -22,12 +22,14 @@ class CarServer(): def start_server(self): server = grpc.server(futures.ThreadPoolExecutor(max_workers=8)) - motorService_pb2_grpc.add_CarControlServicer_to_server(self.create_motor_servicer(), server) + motorService_pb2_grpc.add_CarControlServicer_to_server( + self.create_motor_servicer(), server) SlamController_pb2_grpc.add_SlamControlServicer_to_server( self.create_slam_servicer(), server) lidar_tracker_pb2_grpc.add_PersonTrackingServicer_to_server( self.create_lidar_servicer(), server) - # Disable tls for local testing. + + # Disable tls for local testing for now. # server.add_secure_port('[::]:50051', self.create_credentials()) server.add_insecure_port('[::]:50051') server.start() @@ -44,7 +46,7 @@ class CarServer(): return LidarServicer() def create_credentials(self): - # Relativise this stuff. + # Relativise this stuff. Should add to source directory. pvtKeyPath = '/home/pi/tls/device.key' pvtCertPath = '/home/pi/tls/device.crt' @@ -55,8 +57,7 @@ class CarServer(): if __name__ == '__main__': - vehicle = Vehicle() - server = CarServer(vehicle) + server = CarServer(vehicle_factory.get_vehicle()) # Can't remember why I do this, is it even needed? service_thread = Thread(target=server.start_server) diff --git a/car/src/car/slam/slam_servicer.py b/car/src/car/slam/slam_servicer.py index 7e40669..0431e30 100644 --- a/car/src/car/slam/slam_servicer.py +++ b/car/src/car/slam/slam_servicer.py @@ -1,5 +1,6 @@ import car.slam.SlamController_pb2_grpc as grpc import car.slam.SlamController_pb2 as proto +import car.empty_pb2 as empty import car.slam.slam_streamer as slam from multiprocessing import Process @@ -21,11 +22,11 @@ class SlamServicer(grpc.SlamControlServicer): self.slam.map_meters = request.map_size_meters self.slam_thread = Process(target=self.slam.start) self.slam_thread.start() - return proto.Empty() + return empty.Empty() def stop_streaming(self, request, context): if self.slam_thread is not None: self.slam.stop_scanning() self.slam_thread.join() self.slam = None - return proto.Empty() + return empty.Empty() diff --git a/car/src/car/tracking/devices/factory.py b/car/src/car/tracking/devices/factory.py index 43e1123..65df823 100644 --- a/car/src/car/tracking/devices/factory.py +++ b/car/src/car/tracking/devices/factory.py @@ -1,18 +1,27 @@ -from car.tracking.devices.mock_lidar import MockLidar -import car.tracking.lidar_loader as loader +from .mock_lidar import MockLidar +from .. import lidar_loader as loader +import os -# connection = "TEST" -connection = '/dev/ttyUSB0' +MOCK_DEVICE = "LIDAR_MOCK" +RPLIDAR = "LIDAR_RPLIDAR" -def get_lidar(): - # Need a way to configure this, maybe with environment variables - if connection == 'TEST': - return MockLidar(loader.load_scans_bytes_file("tracking/out.pickle")) - else: + +def get_lidar(device=None, connection='/dev/ttyUSB0'): + actual_device = None + try: + actual_device = device if device is not None else os.environ["CAR_LIDAR"] + except KeyError: + print( + 'No lidar device specified and the CAR_LIDAR environment variable is not set.') + if actual_device == MOCK_DEVICE: + return MockLidar(loader.load_scans_bytes_file("car/tracking/out.pickle")) + elif actual_device == RPLIDAR: try: from rplidar import RPLidar return RPLidar(connection) except ImportError: - print('Could not import RPLidar, using mock with testing data.') - return MockLidar(loader.load_scans_bytes_file("tracking/out.pickle")) - + print('Could not import RPLidar. Have you downloaded rplidar?') + else: + print('No valid lidar device found. Please choose one of ' + + MOCK_DEVICE + ' or ' + RPLIDAR) + return None diff --git a/car/src/car/tracking/lidar_loader.py b/car/src/car/tracking/lidar_loader.py index ced6b1b..60ccf50 100644 --- a/car/src/car/tracking/lidar_loader.py +++ b/car/src/car/tracking/lidar_loader.py @@ -6,11 +6,11 @@ As such, it is useful for testing, to create real lidar data that can be reused later, without needing to connect the lidar. """ -from rplidar import RPLidar import pickle def get_scans(num_scans, device='/dev/ttyUSB0', measurements_per_scan=100): + from rplidar import RPLidar lidar = RPLidar(device) scans = lidar.iter_scans(measurements_per_scan) return [next(scans) for i in range(0, num_scans)]