From 71eeaeb9cdd1cfdc2b2c39766c45654384303fc6 Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Mon, 20 Apr 2020 22:26:52 +0930
Subject: [PATCH] Don't allow client to specify zmq port.

---
 car/src/car/control/gpio/factory.py    |  8 ++------
 car/src/car/slam/slam_servicer.py      |  4 +++-
 car/src/car/tracking/lidar_servicer.py | 14 +++++++++-----
 3 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/car/src/car/control/gpio/factory.py b/car/src/car/control/gpio/factory.py
index 69df0a1..505e2aa 100644
--- a/car/src/car/control/gpio/factory.py
+++ b/car/src/car/control/gpio/factory.py
@@ -3,12 +3,7 @@ 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()
+    ENV_CAR = None if 'CAR_VEHICLE' not in os.environ else os.environ['CAR_VEHICLE']
     if ENV_CAR == "CAR_2D":
         try:
             from .vehicle import Vehicle
@@ -20,3 +15,4 @@ def get_vehicle(motor_pin=19, steering_pin=18):
         return MockVehicle(motor_pin, steering_pin)
     else:
         print('No valid vehicle found. Have you set the CAR_VEHICLE environment variable?')
+        return None
diff --git a/car/src/car/slam/slam_servicer.py b/car/src/car/slam/slam_servicer.py
index 0431e30..54847cd 100644
--- a/car/src/car/slam/slam_servicer.py
+++ b/car/src/car/slam/slam_servicer.py
@@ -3,6 +3,7 @@ import car.slam.SlamController_pb2 as proto
 import car.empty_pb2 as empty
 import car.slam.slam_streamer as slam
 from multiprocessing import Process
+import os
 
 
 class SlamServicer(grpc.SlamControlServicer):
@@ -17,7 +18,8 @@ class SlamServicer(grpc.SlamControlServicer):
         if self.slam_thread is None:
             print('initialising slam_thread')
             # Don't bother creating and starting slam more than once.
-            self.slam.port = request.port
+            self.slam.port = 50052 if "CAR_ZMQ_PORT" not in os.environ else os.environ[
+                'CAR_ZMQ_PORT']
             self.slam.map_pixels = request.map_size_pixels
             self.slam.map_meters = request.map_size_meters
             self.slam_thread = Process(target=self.slam.start)
diff --git a/car/src/car/tracking/lidar_servicer.py b/car/src/car/tracking/lidar_servicer.py
index e9e7491..c987f8c 100644
--- a/car/src/car/tracking/lidar_servicer.py
+++ b/car/src/car/tracking/lidar_servicer.py
@@ -7,35 +7,39 @@ import car.tracking.devices.factory as lidar_factory
 
 from car.messaging import messages
 import car.tracking.algorithms as alg
+import os
+
 
 class LidarServicer(PersonTrackingServicer):
 
     def __init__(self, vehicle=None):
         # TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer.
-        # Also, it would allow creating the service without the lidar being connected. 
+        # Also, it would allow creating the service without the lidar being connected.
         self.cache = LidarCache(lidar_factory.get_lidar(), measurements=100)
         self.cache.add_groups_changed_listener(self)
         self._mFactory = None
-        self._port = None
+        self._port = 50052 if 'CAR_ZMQ_PORT' not in os.environ else os.environ[
+            'CAR_ZMQ_PORT']
         self._vehicle = vehicle
         self._tracked_group = None
 
     def set_tracking_group(self, request, context):
-        self._tracked_group = request.value
+        # Invalid groups should stop tracking
+        self._tracked_group = None if request.value < 0 else request.value
 
     def stop_tracking(self, request, context):
         self.cache.stop_scanning()
 
     def start_tracking(self, request, context):
         """Starts the lidar cache, streaming on the provided port."""
-        self._port = request.value
         self.cache.start_cache()
 
     def onGroupsChanged(self, message):
         if self._mFactory is None:
             # Create the zmq socket in the thread that it will be used, just to be safe.
             self._mFactory = mf.getZmqPubSubStreamer(self._port)
-        self._mFactory.send_message_topic("lidar_map", messages.ProtoMessage(message=message.SerializeToString()))
+        self._mFactory.send_message_topic(
+            "lidar_map", messages.ProtoMessage(message=message.SerializeToString()))
 
         if self._tracked_group is not None and self._vehicle is not None:
             # Update vehicle to correctly follow the tracked group.