diff --git a/donkeycar/parts/dgym.py b/donkeycar/parts/dgym.py index fb067d60a..6f1d37939 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,74 @@ 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 + # 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', + 'speed': 'speed', + 'forward_vel': 'forward_vel', + 'hit': 'hit', + 'gyro': 'gyro', # [x, y, z] + 'accel': 'accel', # [x, y, z] + 'vel': 'vel', # [x, y, z] + 'odom': ["front_left", "front_right", "rear_left", "rear_right"], + 'lidar': 'lidar', + '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): + # 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( + "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 +123,23 @@ 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(): + 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 += out + # if it's a single value or vector + else: + outputs.append(out) + if len(outputs) == 1: return self.frame else: @@ -95,5 +147,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/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..c37ac5ea9 100644 --- a/donkeycar/templates/complete.py +++ b/donkeycar/templates/complete.py @@ -748,30 +748,14 @@ 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'] + outputs = ['cam/image_array'] # modified in DonkeyGymEnv constructor according to the config - 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'] + # 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=threaded) + V.add(gym, inputs=inputs, outputs=outputs, threaded=True) def get_camera(cfg): diff --git a/donkeycar/tests/test_dgym.py b/donkeycar/tests/test_dgym.py new file mode 100644 index 000000000..159a0ce4a --- /dev/null +++ b/donkeycar/tests/test_dgym.py @@ -0,0 +1,318 @@ + +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 + +logger = logging.getLogger(__name__) + + +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 = 0 + 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.socket = None + self.client = None + self.running = True + self.thread = threading.Thread(target=self.run) + self.thread.start() + + logger.info("Test server started") + + def __enter__(self): + return self + + 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() + + self.car_loaded() + + while self.running: + 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() + logger.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.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": 1.7, + "speed": 1.8, + "roll": 1.9, "pitch": 2.0, "yaw": 2.1, + } + self.send(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 + 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): + # 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", + "vel", + "gyro", + "accel", + "front_left", + "front_right", + "rear_left", + "rear_right", + "cte", + "speed", + "orientation", + ]) + + 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, testing it separately + "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", + "vel", + "gyro", + "accel", + "front_left", + "front_right", + "rear_left", + "rear_right", + "cte", + "speed", + "orientation", + ]) + + # 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.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 + ]) + + 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]]) diff --git a/setup.py b/setup.py index 4b84f2cd8..2bd2a3c5f 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', ], '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'])), - ) + )