Implement ego pedestrian sensor functions and testing framework

robot_sf/gym_env/env_util.py

@@ -7,6 +7,7 @@
 
 from gymnasium import spaces
 import numpy as np
+from loguru import logger
 
 from robot_sf.gym_env.env_config import EnvSettings, PedEnvSettings
 from robot_sf.nav.map_config import MapDefinition
@@ -278,19 +279,25 @@ def speed_sensor(r_id=0):
         )
     )
 
-    def ray_sensor():
+    def ray_sensor_ego_ped():
         return lidar_ray_scan(sim.ego_ped.pose, occupancies[1], lidar_config)[0]
 
-    def target_sensor():
+    def target_sensor_ego_ped():
         return target_sensor_obs(
             sim.ego_ped.pose, sim.ego_ped_goal_pos, None
         )  # TODO: What next goal to choose?
 
-    def speed_sensor():
+    def speed_sensor_ego_ped():
         return sim.ego_ped.current_speed
 
     sensor_fusions.append(
-        SensorFusion(ray_sensor, speed_sensor, target_sensor, orig_obs_space[1], False)
+        SensorFusion(
+            ray_sensor_ego_ped,
+            speed_sensor_ego_ped,
+            target_sensor_ego_ped,
+            orig_obs_space[1],
+            False,
+        )
     )  # Ego pedestrian does not have a next goal
 
     # Format: [robot, ego_pedestrian]

tests/env_test.py

@@ -47,3 +47,9 @@ def test_ego_ped_env():
         _, _, done, _, _ = env.step(rand_action)
         if done:
             env.reset()
+
+if __name__ == "__main__":
+    # test_can_create_env()
+    # test_can_return_valid_observation()
+    # test_can_simulate_with_pedestrians()
+    test_ego_ped_env()