Merge remote-tracking branch 'upstream/master'

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

gym_donkeycar/core/client.py

@@ -111,7 +111,7 @@ def proc_msg(self, sock):  # noqa: C901
                     localbuffer += data
 
                     n0 = localbuffer.find("{")
-                    n1 = localbuffer.rfind("}")
+                    n1 = localbuffer.rfind("}\n")
                     if n1 >= 0 and 0 <= n0 < n1:  # there is at least one message :
                         msgs = localbuffer[n0 : n1 + 1].split("\n")
                         localbuffer = localbuffer[n1:]

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}")