Dgym refactor

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,23 +123,28 @@ 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:
             return outputs
 
     def shutdown(self):
         self.running = False
-        time.sleep(0.2)
         self.env.close()

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

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):