Dgym refactor

donkeycar/parts/dgym.py

@@ -48,34 +48,33 @@ def __init__(self, cfg, outputs):
 
         # output keys corresponding to info dict values
         self.info_keys = {
-            'pos': ['pos/x', 'pos/y', 'pos/z'],
+            'pos': 'pos',  # [x, y, z]
             'cte': 'cte',
             'speed': 'speed',
             'forward_vel': 'forward_vel',
             'hit': 'hit',
-            'gyro': ['gyro/x', 'gyro/y', 'gyro/z'],
-            'accel': ['accel/x', 'accel/y', 'accel/z'],
-            'vel': ['vel/x', 'vel/y', 'vel/z'],
-            'odom': ['odom/front_left', 'odom/front_right', 'odom/rear_left', 'odom/rear_right'],
+            'gyro': 'gyro',  # [x, y, z]
+            'accel': 'accel',  # [x, y, z]
+            'vel': 'vel',  # [x, y, z]
+            'odom': 'odom',  # [fl, fr, rl, rr]
             'lidar': 'lidar',
-            'orientation': ['orientation/roll', 'orientation/pitch', 'orientation/yaw'],
+            'orientation': "orientation",  # [roll, pitch, yaw]
             'last_lap_time': 'last_lap_time',
             'lap_count': 'lap_count',
         }
 
         self.output_keys = {}
-
+
+        # fill in the output list according to the config
         try:
             for key, val in cfg.SIM_RECORD.items():
                 if cfg.SIM_RECORD[key]:
                     outputs_key = self.info_keys[key]
-                    if isinstance(outputs_key, list):
-                        outputs += outputs_key
-                    else:
-                        outputs += [outputs_key]
+                    outputs.append(outputs_key)
                     self.output_keys[key] = outputs_key
         except:
-            raise Exception("SIM_RECORD could not be found in config.py. Please add it to your config.py file.")
+            raise Exception(
+                "SIM_RECORD could not be found in config.py. Please add it to your config.py file.")
 
         self.delay = float(cfg.SIM_ARTIFICIAL_LATENCY) / 1000.0
         self.buffer = []
@@ -129,5 +128,4 @@ def run_threaded(self, steering, throttle, brake=None):
 
     def shutdown(self):
         self.running = False
-        time.sleep(0.2)
         self.env.close()

donkeycar/tests/test_dgym.py

@@ -12,6 +12,8 @@
 
 from donkeycar.parts.dgym import DonkeyGymEnv
 
+logger = logging.getLogger(__name__)
+
 
 class Config(object):
     def __init__(self):
@@ -50,35 +52,44 @@ def __init__(self):
         self.host = "127.0.0.1"
         self.port = 9091
 
-        self.thread = threading.Thread(target=self.run, daemon=True)
+        self.socket = None
+        self.client = None
+        self.running = True
+        self.thread = threading.Thread(target=self.run)
         self.thread.start()
 
-        self.running = True
-        self.last_control = None
+        logger.info("Test server started")
+
+    def __enter__(self):
+        return self
 
-        logging.info("Test server started")
+    def car_loaded(self):
+        msg = {"msg_type": "car_loaded"}
+        self.send(msg)
 
     def run(self):
+        """
+        Imitate the donkeysim server with telemetry.
+        """
         self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
         self.socket.bind((self.host, self.port))
         self.socket.listen(1)
         self.client, self.addr = self.socket.accept()
 
-        # send a car_loaded message then start listening
         self.car_loaded()
-        self.listen()
-
-    def car_loaded(self):
-        msg = {"msg_type": "car_loaded"}
-        self.send(msg)
 
-    def listen(self):
         while self.running:
-            data = self.client.recv(1024)
-            if not data:
+            try:
+                data = self.client.recv(1024 * 256)
+                if not data:
+                    self.running = False
+                    break
+            except socket.error:
+                self.running = False
                 break
+
             data = data.decode("utf-8").strip()
-            logging.debug(f"Received: {data}")
+            logger.debug(f"Received: {data}")
 
             # create dummy base64 png image
             img = np.zeros((120, 160, 3), dtype=np.uint8)
@@ -88,43 +99,52 @@ def listen(self):
             msg = {
                 "msg_type": "telemetry",
                 "image": base64.b64encode(encimg).decode("utf-8"),
-                "pos_x": 0.0, "pos_y": 0.0, "pos_z": 0.0,
-                "vel_x": 0.0, "vel_y": 0.0, "vel_z": 0.0,
-                "gyro_x": 0.0, "gyro_y": 0.0, "gyro_z": 0.0,
-                "accel_x": 0.0, "accel_y": 0.0, "accel_z": 0.0,
-                "odom_fl": 0.1, "odom_fr": 0.2, "odom_rl": 0.3, "odom_rr": 0.4,
-                "lidar": [
-                    {"d": 0.0, "rx": 0.0, "ry": 0.0},
-                    {"d": 0.0, "rx": 0.1, "ry": 0.0},
-                    {"d": 0.0, "rx": 0.2, "ry": 0.0},
+                "pos_x": 0.1, "pos_y": 0.2, "pos_z": 0.3,
+                "vel_x": 0.4, "vel_y": 0.5, "vel_z": 0.6,
+                "gyro_x": 0.7, "gyro_y": 0.8, "gyro_z": 0.9,
+                "accel_x": 1.0, "accel_y": 1.1, "accel_z": 1.2,
+                "odom_fl": 1.3, "odom_fr": 1.4, "odom_rl": 1.5, "odom_rr": 1.6,
+                "lidar": [  # simplified lidar data for testing
+                    {"d": 10.0, "rx": 0.0, "ry": 0.0},
+                    {"d": 20.0, "rx": 90.0, "ry": 0.0},
+                    {"d": 30.0, "rx": 180.0, "ry": 0.0},
+                    {"d": 40.0, "rx": 270.0, "ry": 0.0},
                 ],
-                "cte": 0.0,
-                "speed": 0.0,
-                "roll": 0.0, "pitch": 0.0, "yaw": 0.0,
+                "cte": 1.7,
+                "speed": 1.8,
+                "roll": 1.9, "pitch": 2.0, "yaw": 2.1,
             }
             self.send(msg)
-            logging.debug(f"Sent: {msg}")
+            logger.debug(f"Sent: {msg}")
 
     def send(self, msg: dict):
         json_msg = json.dumps(msg)
         self.client.sendall(json_msg.encode("utf-8") + b"\n")
 
     def close(self):
         self.running = False
-        self.socket.close()
-        logging.info("Test server closed")
+        if self.client is not None:
+            self.client.close()
+        if self.socket is not None:
+            self.socket.close()
+
+    def __exit__(self, exc_type, exc_value, traceback):
+        self.close()
 
 
 #
 # python -m unittest donkeycar/tests/test_dgym.py
 #
 class TestDgym(unittest.TestCase):
     def setUp(self):
+        self.gym = None
         self.cfg = Config()
         self.server = Server()
 
     def tearDown(self):
         self.cfg = None
+        if self.gym is not None:
+            self.gym.shutdown()
         self.server.close()
 
     def test_dgym_startup(self):
@@ -151,14 +171,14 @@ def test_dgym_startup(self):
         # check that the output list is correct
         self.assertEqual(outputs, [
             "cam/image_array",
-            "pos/x", "pos/y", "pos/z",
-            "vel/x", "vel/y", "vel/z",
-            "gyro/x", "gyro/y", "gyro/z",
-            "accel/x", "accel/y", "accel/z",
-            "odom/front_left", "odom/front_right", "odom/rear_left", "odom/rear_right",
+            "pos",
+            "vel",
+            "gyro",
+            "accel",
+            "odom",
             "cte",
             "speed",
-            "orientation/roll", "orientation/pitch", "orientation/yaw",
+            "orientation",
         ])
 
         self.gym.run_threaded(0.5, 0.25, brake=0.1)
@@ -172,7 +192,7 @@ def test_dgym_telemetry(self):
             "gyro": True,
             "accel": True,
             "odom": True,
-            "lidar": False,  # disabling lidar for now, need to test as well
+            "lidar": False,  # disabling lidar, testing it separately
             "cte": True,
             "speed": True,
             "orientation": True,
@@ -188,33 +208,104 @@ def test_dgym_telemetry(self):
         # check that the output list is correct
         self.assertEqual(outputs, [
             "cam/image_array",
-            "pos/x", "pos/y", "pos/z",
-            "vel/x", "vel/y", "vel/z",
-            "gyro/x", "gyro/y", "gyro/z",
-            "accel/x", "accel/y", "accel/z",
-            "odom/front_left", "odom/front_right", "odom/rear_left", "odom/rear_right",
+            "pos",
+            "vel",
+            "gyro",
+            "accel",
+            "odom",
             "cte",
             "speed",
-            "orientation/roll", "orientation/pitch", "orientation/yaw",
+            "orientation",
         ])
 
         # check that the telemetry is correct
-        current_frame, _, _, current_info = self.gym.env.step([0.5, 0.25, 0.1])
+        current_frame, _, _, current_info = self.gym.env.step(
+            [0.5, 0.25, 0.1])
         self.gym.frame = current_frame
         self.gym.info = current_info
 
         output_data = self.gym.run_threaded(0.5, 0.25, brake=0.1)
         output_image, output_info = output_data[0], output_data[1:]
 
         self.assertEqual(output_info, [
-            0.0, 0.0, 0.0,
-            0.0, 0.0, 0.0,
-            0.0, 0.0, 0.0,
-            0.0, 0.0, 0.0,
-            0.1, 0.2, 0.3, 0.4,
-            0.0,
-            0.0,
-            0.0, 0.0, 0.0,
+            (0.1, 0.2, 0.3),  # pos
+            (0.4, 0.5, 0.6),  # vel
+            (0.7, 0.8, 0.9),  # gyro
+            (1.0, 1.1, 1.2),  # accel
+            (1.3, 1.4, 1.5, 1.6),  # odom
+            1.7,  # cte
+            1.8,  # speed
+            (1.9, 2.0, 2.1),  # orientation
         ])
 
         self.assertEqual(output_image.shape, current_frame.shape)
+
+    def test_dgym_lidar_not_initialized(self):
+        self.cfg.SIM_RECORD = {
+            "lidar": True,
+        }
+
+        outputs = ["cam/image_array"]
+        self.gym = DonkeyGymEnv(self.cfg, outputs)
+
+        self.assertNotEqual(self.gym.env, None)
+        self.assertEqual(self.gym.action, [0.0, 0.0, 0.0])
+        self.assertEqual(self.gym.running, True)
+
+        # check that the output list is correct
+        self.assertEqual(outputs, [
+            "cam/image_array",
+            "lidar",
+        ])
+
+        # check the telemetry when lidar is not initialized
+        current_frame, _, _, current_info = self.gym.env.step([0.5, 0.25, 0.1])
+        self.gym.frame = current_frame
+        self.gym.info = current_info
+
+        output_data = self.gym.run_threaded(0.5, 0.25, brake=0.1)
+        output_image, output_info = output_data[0], output_data[1:]
+
+        # expected to be empty as we don't have initialized the lidar
+        self.assertEqual(output_info, [[]])
+
+    def test_dgym_lidar_initialized(self):
+        self.cfg.SIM_RECORD = {
+            "lidar": True,
+        }
+
+        self.cfg.GYM_CONF = {
+            "lidar_config": {
+                "deg_per_sweep_inc": "90.0",
+                "deg_ang_down": "0.0",
+                "deg_ang_delta": "-1.0",
+                "num_sweeps_levels": "1",
+                "max_range": "50.0",
+                "noise": "0.4",
+                "offset_x": "0.0", "offset_y": "0.5", "offset_z": "0.5", "rot_x": "0.0",
+            },
+        }
+
+        outputs = ["cam/image_array"]
+        self.gym = DonkeyGymEnv(self.cfg, outputs)
+
+        self.assertNotEqual(self.gym.env, None)
+        self.assertEqual(self.gym.action, [0.0, 0.0, 0.0])
+        self.assertEqual(self.gym.running, True)
+
+        # check that the output list is correct
+        self.assertEqual(outputs, [
+            "cam/image_array",
+            "lidar",
+        ])
+
+        # check the telemetry when lidar is not initialized
+        current_frame, _, _, current_info = self.gym.env.step([0.5, 0.25, 0.1])
+        self.gym.frame = current_frame
+        self.gym.info = current_info
+
+        output_data = self.gym.run_threaded(0.5, 0.25, brake=0.1)
+        output_image, output_info = output_data[0], output_data[1:]
+
+        self.assertEqual(len(output_info[0]), 4)
+        self.assertEqual(output_info, [[10.0, 20.0, 30.0, 40.0]])

setup.py

@@ -78,7 +78,9 @@ def package_files(directory, strip_leading):
               'pandas',
               'pyyaml',
               'plotly',
-              'imgaug'
+              'imgaug',
+              'gym',
+              'gym-donkeycar @ git+ssh://[email protected]/tawnkramer/gym-donkeycar.git@master#egg=gym-donkeycar',
           ],
           'dev': [
               'pytest',
@@ -118,4 +120,4 @@ def package_files(directory, strip_leading):
       ],
       keywords='selfdriving cars donkeycar diyrobocars',
       packages=find_packages(exclude=(['tests', 'docs', 'site', 'env'])),
-    )
+      )