From 4d09cf998d1b20df6802619b515ff83013ce30f5 Mon Sep 17 00:00:00 2001 From: Maxime Ellerbach Date: Mon, 27 Feb 2023 19:04:05 +0100 Subject: [PATCH 1/7] draft at dgym refactor --- donkeycar/parts/dgym.py | 97 +++++++++++++++++++---------- donkeycar/templates/cfg_complete.py | 27 +++++--- donkeycar/templates/complete.py | 26 ++------ 3 files changed, 87 insertions(+), 63 deletions(-) diff --git a/donkeycar/parts/dgym.py b/donkeycar/parts/dgym.py index fb067d60a..ca33f6607 100644 --- a/donkeycar/parts/dgym.py +++ b/donkeycar/parts/dgym.py @@ -9,9 +9,9 @@ def is_exe(fpath): class DonkeyGymEnv(object): - - def __init__(self, sim_path, host="127.0.0.1", port=9091, headless=0, env_name="donkey-generated-track-v0", sync="asynchronous", conf={}, record_location=False, record_gyroaccel=False, record_velocity=False, record_lidar=False, delay=0): - + def __init__(self, cfg, outputs): + sim_path = cfg.DONKEY_SIM_PATH + sim_host = cfg.SIM_HOST if sim_path != "remote": if not os.path.exists(sim_path): raise Exception( @@ -20,28 +20,63 @@ def __init__(self, sim_path, host="127.0.0.1", port=9091, headless=0, env_name=" if not is_exe(sim_path): raise Exception("The path you provided is not an executable.") - conf["exe_path"] = sim_path - conf["host"] = host - conf["port"] = port - conf["guid"] = 0 - conf["frame_skip"] = 1 - self.env = gym.make(env_name, conf=conf) + + gym_conf = cfg.GYM_CONF + gym_conf["exe_path"] = sim_path + gym_conf["host"] = sim_host + gym_conf["port"] = 9091 + gym_conf["frame_skip"] = 1 + + self.env = gym.make(cfg.DONKEY_GYM_ENV_NAME, conf=gym_conf) self.frame = self.env.reset() self.action = [0.0, 0.0, 0.0] self.running = True - self.info = {'pos': (0., 0., 0.), - 'speed': 0, - 'cte': 0, - 'gyro': (0., 0., 0.), - 'accel': (0., 0., 0.), - 'vel': (0., 0., 0.), - 'lidar': []} - self.delay = float(delay) / 1000 - self.record_location = record_location - self.record_gyroaccel = record_gyroaccel - self.record_velocity = record_velocity - self.record_lidar = record_lidar - + self.info = { + 'pos': (0., 0., 0.), + 'cte': 0.0, + 'speed': 0.0, + 'forward_vel': 0.0, + 'hit': False, + 'gyro': (0., 0., 0.), + 'accel': (0., 0., 0.), + 'vel': (0., 0., 0.), + 'odom': (0., 0., 0., 0.), + 'lidar': [], + 'orientation': (0., 0., 0.), + 'last_lap_time': 0.0, + 'lap_count': 0, + } + + # output keys corresponding to info dict values + self.info_keys = { + 'pos': ['pos/x', 'pos/y', 'pos/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'], + 'lidar': 'lidar', + 'orientation': ['orientation/roll', 'orientation/pitch', 'orientation/yaw'], + 'last_lap_time': 'last_lap_time', + 'lap_count': 'lap_count', + } + + self.output_keys = {} + + try: + for key, val in cfg.SIM_RECORD.items(): + if cfg.SIM_RECORD[key]: + outputs_key = self.info_keys[key] + outputs += [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.") + + + self.delay = float(cfg.SIM_ARTIFICIAL_LATENCY) / 1000.0 self.buffer = [] def delay_buffer(self, frame, info): @@ -77,17 +112,15 @@ def run_threaded(self, steering, throttle, brake=None): brake = 0.0 self.action = [steering, throttle, brake] - - # Output Sim-car position information if configured outputs = [self.frame] - if self.record_location: - outputs += self.info['pos'][0], self.info['pos'][1], self.info['pos'][2], self.info['speed'], self.info['cte'] - if self.record_gyroaccel: - outputs += self.info['gyro'][0], self.info['gyro'][1], self.info['gyro'][2], self.info['accel'][0], self.info['accel'][1], self.info['accel'][2] - if self.record_velocity: - outputs += self.info['vel'][0], self.info['vel'][1], self.info['vel'][2] - if self.record_lidar: - outputs += [self.info['lidar']] + + # fill in outputs according to required info + for key, val in self.output_keys.items(): + if isinstance(val, list): + outputs += self.info[key] + else: + outputs += [self.info[key]] + if len(outputs) == 1: return self.frame else: diff --git a/donkeycar/templates/cfg_complete.py b/donkeycar/templates/cfg_complete.py index 4ce6a4c46..2b415be8d 100644 --- a/donkeycar/templates/cfg_complete.py +++ b/donkeycar/templates/cfg_complete.py @@ -567,19 +567,28 @@ DONKEY_GYM = False DONKEY_SIM_PATH = "path to sim" #"/home/tkramer/projects/sdsandbox/sdsim/build/DonkeySimLinux/donkey_sim.x86_64" when racing on virtual-race-league use "remote", or user "remote" when you want to start the sim manually first. DONKEY_GYM_ENV_NAME = "donkey-generated-track-v0" # ("donkey-generated-track-v0"|"donkey-generated-roads-v0"|"donkey-warehouse-v0"|"donkey-avc-sparkfun-v0") -GYM_CONF = { "body_style" : "donkey", "body_rgb" : (128, 128, 128), "car_name" : "car", "font_size" : 100} # body style(donkey|bare|car01) body rgb 0-255 -GYM_CONF["racer_name"] = "Your Name" -GYM_CONF["country"] = "Place" -GYM_CONF["bio"] = "I race robots." +GYM_CONF = { + "body_style" : "donkey", # donkey | bare | car01 + "body_rgb" : (128, 128, 128), # (0-255, 0-255, 0-255) + "car_name" : "car", + "font_size" : 100, +} SIM_HOST = "127.0.0.1" # when racing on virtual-race-league use host "trainmydonkey.com" SIM_ARTIFICIAL_LATENCY = 0 # this is the millisecond latency in controls. Can use useful in emulating the delay when useing a remote server. values of 100 to 400 probably reasonable. -# Save info from Simulator (pln) -SIM_RECORD_LOCATION = False -SIM_RECORD_GYROACCEL= False -SIM_RECORD_VELOCITY = False -SIM_RECORD_LIDAR = False +# indicate which sensors to record +SIM_RECORD = { + "pos": False, + "vel": False, + "gyro": False, + "accel": False, + "odom": False, + "lidar": False, + "cte": False, + "speed": False, + "orientation": False, +} #publish camera over network #This is used to create a tcp service to publish the camera feed diff --git a/donkeycar/templates/complete.py b/donkeycar/templates/complete.py index 551821cd1..ec4d65bca 100644 --- a/donkeycar/templates/complete.py +++ b/donkeycar/templates/complete.py @@ -748,30 +748,12 @@ def add_simulator(V, cfg): # TODO: the simulation outputs conflict with imu, odometry, kinematics pose estimation and T265 outputs; make them work together. if cfg.DONKEY_GYM: from donkeycar.parts.dgym import DonkeyGymEnv - # rbx - gym = DonkeyGymEnv(cfg.DONKEY_SIM_PATH, host=cfg.SIM_HOST, env_name=cfg.DONKEY_GYM_ENV_NAME, conf=cfg.GYM_CONF, - record_location=cfg.SIM_RECORD_LOCATION, record_gyroaccel=cfg.SIM_RECORD_GYROACCEL, - record_velocity=cfg.SIM_RECORD_VELOCITY, record_lidar=cfg.SIM_RECORD_LIDAR, - # record_distance=cfg.SIM_RECORD_DISTANCE, record_orientation=cfg.SIM_RECORD_ORIENTATION, - delay=cfg.SIM_ARTIFICIAL_LATENCY) - threaded = True + inputs = ['steering', 'throttle'] - outputs = ['cam/image_array'] - - if cfg.SIM_RECORD_LOCATION: - outputs += ['pos/pos_x', 'pos/pos_y', 'pos/pos_z', 'pos/speed', 'pos/cte'] - if cfg.SIM_RECORD_GYROACCEL: - outputs += ['gyro/gyro_x', 'gyro/gyro_y', 'gyro/gyro_z', 'accel/accel_x', 'accel/accel_y', 'accel/accel_z'] - if cfg.SIM_RECORD_VELOCITY: - outputs += ['vel/vel_x', 'vel/vel_y', 'vel/vel_z'] - if cfg.SIM_RECORD_LIDAR: - outputs += ['lidar/dist_array'] - # if cfg.SIM_RECORD_DISTANCE: - # outputs += ['dist/left', 'dist/right'] - # if cfg.SIM_RECORD_ORIENTATION: - # outputs += ['roll', 'pitch', 'yaw'] + outputs = ['cam/image_array'] # modified in DonkeyGymEnv constructor according to the config + gym = DonkeyGymEnv(cfg, outputs) - V.add(gym, inputs=inputs, outputs=outputs, threaded=threaded) + V.add(gym, inputs=inputs, outputs=outputs, threaded=True) def get_camera(cfg): From aa012178f2f0ae5abbb61e56bd6f3c68ea0d3e4d Mon Sep 17 00:00:00 2001 From: Maxime Ellerbach Date: Mon, 27 Feb 2023 19:07:33 +0100 Subject: [PATCH 2/7] linting --- donkeycar/parts/dgym.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/donkeycar/parts/dgym.py b/donkeycar/parts/dgym.py index ca33f6607..f68192e8a 100644 --- a/donkeycar/parts/dgym.py +++ b/donkeycar/parts/dgym.py @@ -20,7 +20,6 @@ def __init__(self, cfg, outputs): if not is_exe(sim_path): raise Exception("The path you provided is not an executable.") - gym_conf = cfg.GYM_CONF gym_conf["exe_path"] = sim_path gym_conf["host"] = sim_host @@ -65,7 +64,7 @@ def __init__(self, cfg, outputs): } self.output_keys = {} - + try: for key, val in cfg.SIM_RECORD.items(): if cfg.SIM_RECORD[key]: @@ -75,8 +74,7 @@ def __init__(self, cfg, outputs): except: 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.delay = float(cfg.SIM_ARTIFICIAL_LATENCY) / 1000.0 self.buffer = [] def delay_buffer(self, frame, info): @@ -120,7 +118,7 @@ def run_threaded(self, steering, throttle, brake=None): outputs += self.info[key] else: outputs += [self.info[key]] - + if len(outputs) == 1: return self.frame else: From 9600da79f743148e1e005772bedba240015004ec Mon Sep 17 00:00:00 2001 From: Maxime Ellerbach Date: Fri, 3 Mar 2023 11:29:12 +0100 Subject: [PATCH 3/7] added tests for dgym --- donkeycar/parts/dgym.py | 5 +- donkeycar/tests/test_dgym.py | 220 +++++++++++++++++++++++++++++++++++ 2 files changed, 224 insertions(+), 1 deletion(-) create mode 100644 donkeycar/tests/test_dgym.py diff --git a/donkeycar/parts/dgym.py b/donkeycar/parts/dgym.py index f68192e8a..fab183bef 100644 --- a/donkeycar/parts/dgym.py +++ b/donkeycar/parts/dgym.py @@ -69,7 +69,10 @@ def __init__(self, cfg, outputs): for key, val in cfg.SIM_RECORD.items(): if cfg.SIM_RECORD[key]: outputs_key = self.info_keys[key] - outputs += [outputs_key] + if isinstance(outputs_key, list): + outputs += outputs_key + else: + outputs += [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.") diff --git a/donkeycar/tests/test_dgym.py b/donkeycar/tests/test_dgym.py new file mode 100644 index 000000000..a5a1ecde5 --- /dev/null +++ b/donkeycar/tests/test_dgym.py @@ -0,0 +1,220 @@ + +import base64 +import json +import logging +import socket +import threading +import time +import unittest + +import numpy as np +import cv2 + +from donkeycar.parts.dgym import DonkeyGymEnv + + +class Config(object): + def __init__(self): + self.DONKEY_GYM = True + self.DONKEY_SIM_PATH = "remote" + self.DONKEY_GYM_ENV_NAME = "donkey-generated-track-v0" + self.SIM_HOST = "127.0.0.1" + self.SIM_ARTIFICIAL_LATENCY = 9091 + self.SIM_RECORD = { + "pos": False, + "vel": False, + "gyro": False, + "accel": False, + "odom": False, + "lidar": False, + "cte": False, + "speed": False, + "orientation": False, + } + + self.GYM_CONF = { + "body_style": "donkey", + "body_rgb": (128, 128, 128), + "car_name": "donkey", + "font_size": 100, + } + + +class Server(object): + """ + A simple TCP server that listens for a connection. + Used to test the DonkeyGymEnv class. + """ + + def __init__(self): + self.host = "127.0.0.1" + self.port = 9091 + + self.thread = threading.Thread(target=self.run, daemon=True) + self.thread.start() + + self.running = True + self.last_control = None + + logging.info("Test server started") + + def run(self): + 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: + break + data = data.decode("utf-8").strip() + logging.debug(f"Received: {data}") + + # create dummy base64 png image + img = np.zeros((120, 160, 3), dtype=np.uint8) + _, encimg = cv2.imencode(".png", img) + + # send dummy telemetry + 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}, + ], + "cte": 0.0, + "speed": 0.0, + "roll": 0.0, "pitch": 0.0, "yaw": 0.0, + } + self.send(msg) + logging.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") + + +# +# python -m unittest donkeycar/tests/test_dgym.py +# +class TestDgym(unittest.TestCase): + def setUp(self): + self.cfg = Config() + self.server = Server() + + def tearDown(self): + self.cfg = None + self.server.close() + + def test_dgym_startup(self): + # order of these keys does matter as they determine the order of the output list + self.cfg.SIM_RECORD = { + "pos": True, + "vel": True, + "gyro": True, + "accel": True, + "odom": True, + "lidar": False, # disabling lidar for now, need to test as well + "cte": True, + "speed": True, + "orientation": 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", + "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", + "cte", + "speed", + "orientation/roll", "orientation/pitch", "orientation/yaw", + ]) + + self.gym.run_threaded(0.5, 0.25, brake=0.1) + self.assertEqual(self.gym.action, [0.5, 0.25, 0.1]) + + def test_dgym_telemetry(self): + # order of these keys does matter as they determine the order of the output list + self.cfg.SIM_RECORD = { + "pos": True, + "vel": True, + "gyro": True, + "accel": True, + "odom": True, + "lidar": False, # disabling lidar for now, need to test as well + "cte": True, + "speed": True, + "orientation": 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", + "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", + "cte", + "speed", + "orientation/roll", "orientation/pitch", "orientation/yaw", + ]) + + # check that the telemetry is correct + 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, + ]) + + self.assertEqual(output_image.shape, current_frame.shape) From 6da69ae56a3e9030160ee3cfbe1ae2eda9bc247e Mon Sep 17 00:00:00 2001 From: Maxime Ellerbach Date: Wed, 8 Mar 2023 00:16:18 +0100 Subject: [PATCH 4/7] - added tests for lidar - output vectors instead of single components --- donkeycar/parts/dgym.py | 24 ++--- donkeycar/tests/test_dgym.py | 193 ++++++++++++++++++++++++++--------- 2 files changed, 153 insertions(+), 64 deletions(-) diff --git a/donkeycar/parts/dgym.py b/donkeycar/parts/dgym.py index fab183bef..423da6b61 100644 --- a/donkeycar/parts/dgym.py +++ b/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() diff --git a/donkeycar/tests/test_dgym.py b/donkeycar/tests/test_dgym.py index a5a1ecde5..0545958d3 100644 --- a/donkeycar/tests/test_dgym.py +++ b/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,22 +99,23 @@ 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) @@ -111,8 +123,13 @@ def send(self, msg: dict): 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() # @@ -120,11 +137,14 @@ def close(self): # 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,18 +208,19 @@ 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 @@ -207,14 +228,84 @@ def test_dgym_telemetry(self): 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]]) From 49a6af5e16bd00ec5acf2711b56579a6136f38af Mon Sep 17 00:00:00 2001 From: Maxime Ellerbach Date: Wed, 8 Mar 2023 01:18:23 +0100 Subject: [PATCH 5/7] trying to add gym and gym-donkeycar to setup.py --- setup.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/setup.py b/setup.py index 4b84f2cd8..8634676d6 100644 --- a/setup.py +++ b/setup.py @@ -78,7 +78,9 @@ def package_files(directory, strip_leading): 'pandas', 'pyyaml', 'plotly', - 'imgaug' + 'imgaug', + 'gym', + 'gym-donkeycar @ git+ssh://git@github.com/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'])), - ) + ) From 0b3c70397c2917035ee283335755d9e5e0d30696 Mon Sep 17 00:00:00 2001 From: Maxime Ellerbach Date: Wed, 8 Mar 2023 01:42:12 +0100 Subject: [PATCH 6/7] reverting some changes for output keys --- donkeycar/parts/dgym.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/donkeycar/parts/dgym.py b/donkeycar/parts/dgym.py index 423da6b61..6987fc081 100644 --- a/donkeycar/parts/dgym.py +++ b/donkeycar/parts/dgym.py @@ -47,6 +47,12 @@ def __init__(self, cfg, outputs): } # output keys corresponding to info dict values + # used to map gym info names to donkeycar outputs + # + # can also map an iterable value to multiple outputs + # e.g. 'odom': ['fl', 'fr', 'rl', 'rr'] + # This will map the 'odom' key in the info dict to 4 different outputs with the names 'fl', 'fr', 'rl', 'rr' + # self.info_keys = { 'pos': 'pos', # [x, y, z] 'cte': 'cte', @@ -64,13 +70,18 @@ def __init__(self, cfg, outputs): } 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] - outputs.append(outputs_key) + if isinstance(outputs_key, list): + # if it's a list, add each element + outputs += outputs_key + else: + # otherwise, add the key + outputs.append(outputs_key) self.output_keys[key] = outputs_key except: raise Exception( From fbbc1fa64ae0697649abc8cf5e106fb2126f81f1 Mon Sep 17 00:00:00 2001 From: Maxime Ellerbach Date: Sat, 11 Mar 2023 12:56:44 +0100 Subject: [PATCH 7/7] changed tuples to lists + split odom into 4 values --- donkeycar/parts/dgym.py | 14 +++++++++++--- donkeycar/templates/complete.py | 2 ++ donkeycar/tests/test_dgym.py | 25 ++++++++++++++++--------- setup.py | 2 +- 4 files changed, 30 insertions(+), 13 deletions(-) diff --git a/donkeycar/parts/dgym.py b/donkeycar/parts/dgym.py index 6987fc081..6f1d37939 100644 --- a/donkeycar/parts/dgym.py +++ b/donkeycar/parts/dgym.py @@ -62,7 +62,7 @@ def __init__(self, cfg, outputs): 'gyro': 'gyro', # [x, y, z] 'accel': 'accel', # [x, y, z] 'vel': 'vel', # [x, y, z] - 'odom': 'odom', # [fl, fr, rl, rr] + 'odom': ["front_left", "front_right", "rear_left", "rear_right"], 'lidar': 'lidar', 'orientation': "orientation", # [roll, pitch, yaw] 'last_lap_time': 'last_lap_time', @@ -127,10 +127,18 @@ def run_threaded(self, steering, throttle, brake=None): # fill in outputs according to required info for key, val in self.output_keys.items(): + out = self.info[key] + + # convert out to a list if it's not already + if isinstance(out, tuple): + out = list(out) + + # if it's a list (multiple outputs from a single vector) if isinstance(val, list): - outputs += self.info[key] + outputs += out + # if it's a single value or vector else: - outputs += [self.info[key]] + outputs.append(out) if len(outputs) == 1: return self.frame diff --git a/donkeycar/templates/complete.py b/donkeycar/templates/complete.py index ec4d65bca..c37ac5ea9 100644 --- a/donkeycar/templates/complete.py +++ b/donkeycar/templates/complete.py @@ -751,6 +751,8 @@ def add_simulator(V, cfg): inputs = ['steering', 'throttle'] outputs = ['cam/image_array'] # modified in DonkeyGymEnv constructor according to the config + + # the outputs list is modified in the constructor according to the SIM_RECORD config dict gym = DonkeyGymEnv(cfg, outputs) V.add(gym, inputs=inputs, outputs=outputs, threaded=True) diff --git a/donkeycar/tests/test_dgym.py b/donkeycar/tests/test_dgym.py index 0545958d3..159a0ce4a 100644 --- a/donkeycar/tests/test_dgym.py +++ b/donkeycar/tests/test_dgym.py @@ -20,8 +20,9 @@ def __init__(self): self.DONKEY_GYM = True self.DONKEY_SIM_PATH = "remote" self.DONKEY_GYM_ENV_NAME = "donkey-generated-track-v0" + self.SIM_HOST = "127.0.0.1" - self.SIM_ARTIFICIAL_LATENCY = 9091 + self.SIM_ARTIFICIAL_LATENCY = 0 self.SIM_RECORD = { "pos": False, "vel": False, @@ -175,7 +176,10 @@ def test_dgym_startup(self): "vel", "gyro", "accel", - "odom", + "front_left", + "front_right", + "rear_left", + "rear_right", "cte", "speed", "orientation", @@ -212,7 +216,10 @@ def test_dgym_telemetry(self): "vel", "gyro", "accel", - "odom", + "front_left", + "front_right", + "rear_left", + "rear_right", "cte", "speed", "orientation", @@ -228,14 +235,14 @@ def test_dgym_telemetry(self): output_image, output_info = output_data[0], output_data[1:] self.assertEqual(output_info, [ - (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 + [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 (front_left, front_right, rear_left and rear_right) 1.7, # cte 1.8, # speed - (1.9, 2.0, 2.1), # orientation + [1.9, 2.0, 2.1], # orientation ]) self.assertEqual(output_image.shape, current_frame.shape) diff --git a/setup.py b/setup.py index 8634676d6..2bd2a3c5f 100644 --- a/setup.py +++ b/setup.py @@ -80,7 +80,7 @@ def package_files(directory, strip_leading): 'plotly', 'imgaug', 'gym', - 'gym-donkeycar @ git+ssh://git@github.com/tawnkramer/gym-donkeycar.git@master#egg=gym-donkeycar', + 'gym-donkeycar @ git+ssh://git@github.com/tawnkramer/gym-donkeycar.git', ], 'dev': [ 'pytest',