Merge pull request #66 from Maximellerbach/fix-minor-issues

Lidar processing
tawnkramer · Apr 21, 2021 · f1e05ca · f1e05ca
2 parents 862d7b1 + 00e0982
commit f1e05ca
Show file tree

Hide file tree

Showing 6 changed files with 54 additions and 52 deletions.
diff --git a/docs/conf.py b/docs/conf.py
@@ -125,7 +125,7 @@
 # (source start file, target name, title, author, documentclass
 # [howto, manual, or own class]).
 latex_documents = [
-    (master_doc, "gym_donkeycar.tex", u"OpenAI Gym Environments for Donkey CarDocumentation", u"Leigh Johnson", "manual"),
+    (master_doc, "gym_donkeycar.tex", u"OpenAI Gym Environments for Donkey CarDocumentation", u"Leigh Johnson", "manual")
 ]
 
 
@@ -150,5 +150,5 @@
         "gym_donkeycar",
         "One line description of project.",
         "Miscellaneous",
-    ),
+    )
 ]
diff --git a/examples/gym_test.py b/examples/gym_test.py
@@ -10,6 +10,8 @@
 
 import gym
 
+import gym_donkeycar
+
 NUM_EPISODES = 3
 MAX_TIME_STEPS = 1000
 
@@ -100,6 +102,16 @@ def exit_scene(env):
         "guid": str(uuid.uuid4()),
         "start_delay": 1,
         "max_cte": 5,
+        "degPerSweepInc": 2.0,
+        "degAngDown": 0.0,
+        "degAngDelta": -1.0,
+        "numSweepsLevels": 1,
+        "maxRange": 50.0,
+        "noise": 0.4,
+        "offset_x": 0.0,
+        "offset_y": 0.5,
+        "offset_z": 0.5,
+        "rot_x": 0.0,
     }
 
     if args.env_name == "all":

diff --git a/gym_donkeycar/__init__.py b/gym_donkeycar/__init__.py
@@ -27,52 +27,22 @@
 __email__ = "[email protected]"
 __version__ = __version__
 
-register(
-    id="donkey-generated-roads-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:GeneratedRoadsEnv",
-)
+register(id="donkey-generated-roads-v0", entry_point="gym_donkeycar.envs.donkey_env:GeneratedRoadsEnv")
 
-register(
-    id="donkey-warehouse-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:WarehouseEnv",
-)
+register(id="donkey-warehouse-v0", entry_point="gym_donkeycar.envs.donkey_env:WarehouseEnv")
 
-register(
-    id="donkey-avc-sparkfun-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:AvcSparkfunEnv",
-)
+register(id="donkey-avc-sparkfun-v0", entry_point="gym_donkeycar.envs.donkey_env:AvcSparkfunEnv")
 
-register(
-    id="donkey-generated-track-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:GeneratedTrackEnv",
-)
+register(id="donkey-generated-track-v0", entry_point="gym_donkeycar.envs.donkey_env:GeneratedTrackEnv")
 
-register(
-    id="donkey-mountain-track-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:MountainTrackEnv",
-)
+register(id="donkey-mountain-track-v0", entry_point="gym_donkeycar.envs.donkey_env:MountainTrackEnv")
 
-register(
-    id="donkey-roboracingleague-track-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:RoboRacingLeagueTrackEnv",
-)
+register(id="donkey-roboracingleague-track-v0", entry_point="gym_donkeycar.envs.donkey_env:RoboRacingLeagueTrackEnv")
 
-register(
-    id="donkey-waveshare-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:WaveshareEnv",
-)
+register(id="donkey-waveshare-v0", entry_point="gym_donkeycar.envs.donkey_env:WaveshareEnv")
 
-register(
-    id="donkey-minimonaco-track-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:MiniMonacoEnv",
-)
+register(id="donkey-minimonaco-track-v0", entry_point="gym_donkeycar.envs.donkey_env:MiniMonacoEnv")
 
-register(
-    id="donkey-warren-track-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:WarrenTrackEnv",
-)
+register(id="donkey-warren-track-v0", entry_point="gym_donkeycar.envs.donkey_env:WarrenTrackEnv")
 
-register(
-    id="donkey-thunderhill-track-v0",
-    entry_point="gym_donkeycar.envs.donkey_env:ThunderhillTrackEnv",
-)
+register(id="donkey-thunderhill-track-v0", entry_point="gym_donkeycar.envs.donkey_env:ThunderhillTrackEnv")
diff --git a/gym_donkeycar/envs/donkey_env.py b/gym_donkeycar/envs/donkey_env.py
@@ -39,9 +39,7 @@ class DonkeyEnv(gym.Env):
     OpenAI Gym Environment for Donkey
     """
 
-    metadata = {
-        "render.modes": ["human", "rgb_array"],
-    }
+    metadata = {"render.modes": ["human", "rgb_array"]}
 
     ACTION_NAMES = ["steer", "throttle"]
     STEER_LIMIT_LEFT = -1.0

diff --git a/gym_donkeycar/envs/donkey_sim.py b/gym_donkeycar/envs/donkey_sim.py
@@ -128,6 +128,11 @@ def __init__(self, conf):
         self.pitch = 0.0
         self.yaw = 0.0
 
+        # variables required for lidar points decoding into array format
+        self.lidar_deg_per_sweep_inc = 1
+        self.lidar_num_sweep_levels = 1
+        self.lidar_deg_ang_delta = 1
+
     def on_connect(self, client):
         logger.debug("socket connected")
         self.client = client
@@ -190,7 +195,7 @@ def send_config(self, conf):
                 ],
             )
             self.send_cam_config(**cam_config)
-            logger.info("done sending cam config.", cam_config)
+            logger.info(f"done sending cam config. {cam_config}")
         except:
             logger.info("sending cam config FAILED.")
 
@@ -211,7 +216,7 @@ def send_config(self, conf):
                 ],
             )
             self.send_lidar_config(**lidar_config)
-            logger.info("done sending lidar config.", lidar_config)
+            logger.info(f"done sending lidar config., {lidar_config}")
         except:
             logger.info("sending lidar config FAILED.")
         logger.info("done sending car config.")
@@ -367,7 +372,7 @@ def on_telemetry(self, data):
             self.cte = data["cte"]
 
         if "lidar" in data:
-            self.lidar = data["lidar"]
+            self.lidar = self.process_lidar_packet(data["lidar"])
 
         # don't update hit once session over
         if self.over:
@@ -580,6 +585,26 @@ def send_lidar_config(
         self.blocking_send(msg)
         time.sleep(0.1)
 
+        self.lidar_deg_per_sweep_inc = float(degPerSweepInc)
+        self.lidar_num_sweep_levels = int(numSweepsLevels)
+        self.lidar_deg_ang_delta = float(degAngDelta)
+
+    def process_lidar_packet(self, lidar_info):
+        point_per_sweep = int(360 / self.lidar_deg_per_sweep_inc)
+        points_num = round(abs(self.lidar_num_sweep_levels * point_per_sweep))
+        reconstructed_lidar_info = [-1 for _ in range(points_num)]  # we chose -1 to be the "None" value
+
+        for point in lidar_info:
+            rx = point["rx"]
+            ry = point["ry"]
+            d = point["d"]
+
+            x_index = round(abs(rx / self.lidar_deg_per_sweep_inc))
+            y_index = round(abs(ry / self.lidar_deg_ang_delta))
+
+            reconstructed_lidar_info[point_per_sweep * y_index + x_index] = d
+        return np.array(reconstructed_lidar_info)
+
     def blocking_send(self, msg):
         if self.client is None:
             logger.debug(f"skiping: \n {msg}")

diff --git a/gym_donkeycar/test/client.test.py b/gym_donkeycar/test/client.test.py
@@ -61,10 +61,7 @@ def loop(self):
 
 
 class SUT(SDClient):
-    def __init__(
-        self,
-        address,
-    ):
+    def __init__(self, address):
         super().__init__(*address, poll_socket_sleep_time=0.01)
         self.receivedMsg = None
         self.receivedCount = 0