add lidar rays and ped velocity augmentations

demo_offensive.py

@@ -7,7 +7,7 @@
 
 def training():
     env_config = EnvSettings(
-        sim_config=SimulationSettings(difficulty=0, ped_density_by_difficulty=[0.06]),
+        sim_config=SimulationSettings(difficulty=0, ped_density_by_difficulty=[0.02]),
         robot_config=BicycleDriveSettings(radius=0.5, max_accel=3.0, allow_backwards=True))
     env = RobotEnv(env_config, debug=True)
     model = PPO.load("./model/run_043", env=env)

robot_sf/robot_env.py

@@ -177,7 +177,7 @@ def init_collision_and_sensors(
     sensor_fusions: List[SensorFusion] = []
     for r_id in range(num_robots):
         ray_sensor = lambda r_id=r_id: lidar_ray_scan(
-            sim.robots[r_id].pose, occupancies[r_id], lidar_config)
+            sim.robots[r_id].pose, occupancies[r_id], lidar_config)[0]
         target_sensor = lambda r_id=r_id: target_sensor_obs(
             sim.robots[r_id].pose, sim.goal_pos[r_id], sim.next_goal_pos[r_id])
         speed_sensor = lambda r_id=r_id: sim.robots[r_id].current_speed
@@ -208,6 +208,7 @@ def __init__(
             reward_func: Callable[[dict], float] = simple_reward,
             debug: bool = False):
 
+        self.env_config = env_config
         map_def = env_config.map_pool.map_defs[0] # info: only use first map
         self.action_space, self.observation_space, orig_obs_space = init_spaces(env_config, map_def)
 
@@ -251,9 +252,19 @@ def render(self):
         action = None if not self.last_action else VisualizableAction(
             self.simulator.robot_poses[0], self.last_action, self.simulator.goal_pos[0])
 
+        robot_pos = self.simulator.robot_poses[0][0]
+        distances, directions = lidar_ray_scan(
+            self.simulator.robot_poses[0], self.state.occupancy, self.env_config.lidar_config)
+        ray_vecs = zip(np.cos(directions) * distances, np.sin(directions) * distances)
+        ray_vecs_np = np.array(
+            [[[robot_pos[0], robot_pos[1]], [robot_pos[0] + x, robot_pos[1] + y]] for x, y in ray_vecs])
+        ped_actions = zip(self.simulator.pysf_sim.peds.pos(),
+                          self.simulator.pysf_sim.peds.pos() + self.simulator.pysf_sim.peds.vel() * 2)
+        ped_actions_np = np.array([[pos, vel] for pos, vel in ped_actions])
+
         state = VisualizableSimState(
             self.state.timestep, action, self.simulator.robot_poses[0],
-            deepcopy(self.simulator.ped_pos))
+            deepcopy(self.simulator.ped_pos), ray_vecs_np, ped_actions_np)
 
         self.sim_ui.render(state)
 

robot_sf/sensor/range_sensor.py

@@ -194,7 +194,7 @@ def range_postprocessing(out_ranges: np.ndarray, scan_noise: np.ndarray, max_sca
 def lidar_ray_scan(
         pose: RobotPose,
         occ: ContinuousOccupancy,
-        settings: LidarScannerSettings) -> np.ndarray:
+        settings: LidarScannerSettings) -> Tuple[np.ndarray, np.ndarray]:
     """Representing a simulated radial LiDAR scanner operating
     in a 2D plane on a continuous occupancy with explicit objects.
 
@@ -217,7 +217,7 @@ def lidar_ray_scan(
         (pos_x, pos_y), obstacles, scan_dist, ped_pos,
         occ.ped_radius, ray_angles)
     range_postprocessing(ranges, scan_noise, scan_dist)
-    return ranges
+    return ranges, ray_angles
 
 
 def lidar_sensor_space(num_rays: int, max_scan_dist: float) -> spaces.Box:

robot_sf/sim/sim_view.py

@@ -28,7 +28,9 @@
 ROBOT_COLOR = (0, 0, 200)
 COLLISION_COLOR = (200, 0, 0)
 ROBOT_ACTION_COLOR = (65, 105, 225)
+PED_ACTION_COLOR = (255, 50, 50)
 ROBOT_GOAL_COLOR = (0, 204, 102)
+ROBOT_LIDAR_COLOR = (238, 160, 238, 128)
 TEXT_COLOR = (0, 0, 0)
 
 
@@ -47,6 +49,8 @@ class VisualizableSimState:
     action: Union[VisualizableAction, None]
     robot_pose: RobotPose
     pedestrian_positions: np.ndarray
+    ray_vecs: np.ndarray
+    ped_actions: np.ndarray
     # obstacles: List[Obstacle]
 
 
@@ -134,15 +138,18 @@ def render(self, state: VisualizableSimState):
                 exit()
         if self.size_changed:
             self._resize_window()
+            self.size_changed = False
 
         state, offset = self._zoom_camera(state)
         self.screen.fill(BACKGROUND_COLOR)
         self._draw_obstacles(offset)
+        self._augment_lidar(state.ray_vecs)
+        self._augment_ped_actions(state.ped_actions)
         if state.action:
-            self._augment_action_vector(state.action)
+            self._augment_robot_action(state.action)
             self._augment_goal_position(state.action.robot_goal)
-        self._draw_robot(state.robot_pose)
         self._draw_pedestrians(state.pedestrian_positions)
+        self._draw_robot(state.robot_pose)
         self._augment_timestep(state.timestep)
         pygame.display.update()
 
@@ -151,7 +158,6 @@ def _resize_window(self):
         self.screen = pygame.display.set_mode(
             (self.width, self.height), pygame.RESIZABLE)
         self.screen.blit(old_surface, (0, 0))
-        self.size_changed = False
 
     def _zoom_camera(self, state: VisualizableSimState) \
             -> Tuple[VisualizableSimState, Tuple[float, float]]:
@@ -160,6 +166,10 @@ def _zoom_camera(self, state: VisualizableSimState) \
         y_offset = r_y * self.scaling - self.height / 2
         state.pedestrian_positions *= self.scaling
         state.pedestrian_positions -= [x_offset, y_offset]
+        state.ped_actions *= self.scaling
+        state.ped_actions -= [x_offset, y_offset]
+        state.ray_vecs *= self.scaling
+        state.ray_vecs -= [x_offset, y_offset]
         state.robot_pose = ((
             state.robot_pose[0][0] * self.scaling - x_offset,
             state.robot_pose[0][1] * self.scaling - y_offset),
@@ -191,7 +201,11 @@ def _augment_goal_position(self, robot_goal: Vec2D):
         # TODO: display pedestrians with an image instead of a circle
         pygame.draw.circle(self.screen, ROBOT_GOAL_COLOR, robot_goal, self.goal_radius * self.scaling)
 
-    def _augment_action_vector(self, action: VisualizableAction):
+    def _augment_lidar(self, ray_vecs: np.ndarray):
+        for p1, p2 in ray_vecs:
+            pygame.draw.line(self.screen, ROBOT_LIDAR_COLOR, p1, p2)
+
+    def _augment_robot_action(self, action: VisualizableAction):
         r_x, r_y = action.robot_pose[0]
         vec_length, vec_orient = action.robot_action[0] * self.scaling * 3, action.robot_pose[1]
 
@@ -202,7 +216,11 @@ def add_vec(v_1: Vec2D, v_2: Vec2D) -> Vec2D:
             return v_1[0] + v_2[0], v_1[1] + v_2[1]
 
         vec_x, vec_y = add_vec((r_x, r_y), from_polar(vec_length, vec_orient))
-        pygame.draw.line(self.screen, ROBOT_ACTION_COLOR, (r_x, r_y), (vec_x, vec_y))
+        pygame.draw.line(self.screen, ROBOT_ACTION_COLOR, (r_x, r_y), (vec_x, vec_y), width=3)
+
+    def _augment_ped_actions(self, ped_actions: np.ndarray):
+        for p1, p2 in ped_actions:
+            pygame.draw.line(self.screen, PED_ACTION_COLOR, p1, p2, width=3)
 
     def _augment_timestep(self, timestep: int):
         # TODO: show map name as well

tests/lidar_sensor_obstacle_test.py

@@ -27,7 +27,7 @@ def test_scanner_detects_single_obstacle_orthogonal_orientation():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: obstacles, lambda: np.array([[]]))
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), pi), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), pi), occupancy, settings)
 
     exp_dist = 2
     assert scan.shape[0] == lidar_n_rays
@@ -40,7 +40,7 @@ def test_scanner_detects_obstacle_other_orientation_superpositioned():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: obstacles, lambda: np.array([[]]))
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), pi), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), pi), occupancy, settings)
 
     exp_dist = 0
     assert scan.shape[0] == lidar_n_rays
@@ -53,7 +53,7 @@ def test_scanner_ignores_obstacle_same_orientation_superpositioned():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: obstacles, lambda: np.array([[]]))
     settings = LidarScannerSettings(max_scan_dist, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), pi), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), pi), occupancy, settings)
 
     # info: obstacles don't have a body, cannot collide
     exp_dist = max_scan_dist
@@ -67,7 +67,7 @@ def test_scanner_ignores_obstacle_same_orientation_not_superpositioned():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: obstacles, lambda: np.array([[]]))
     settings = LidarScannerSettings(max_scan_dist, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), pi), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), pi), occupancy, settings)
 
     exp_dist = max_scan_dist
     assert scan.shape[0] == lidar_n_rays
@@ -87,7 +87,7 @@ def test_scanner_detects_multiple_equidist_obstacles_from_center():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: obstacles, lambda: np.array([[]]))
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), 0), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), 0), occupancy, settings)
 
     exp_dist = 2.0
     assert scan.shape[0] == lidar_n_rays
@@ -113,7 +113,7 @@ def test_scanner_detects_multiple_equidist_obstacles_randomly_shifted():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: obstacles, lambda: np.array([[]]))
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((shift_x, shift_y), 0), occupancy, settings)
+    scan, _ = lidar_ray_scan(((shift_x, shift_y), 0), occupancy, settings)
 
     exp_dist = 2.0
     assert scan.shape[0] == lidar_n_rays
@@ -126,7 +126,7 @@ def test_scanner_detects_max_range_when_nothing_found():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: np.array([[]]), lambda: np.array([[]]))
     settings = LidarScannerSettings(max_scan_range, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), 0), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), 0), occupancy, settings)
 
     exp_dist = max_scan_range
     assert scan.shape[0] == lidar_n_rays
@@ -139,7 +139,7 @@ def test_scanner_detects_only_closest_obstacle():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: obstacles, lambda: np.array([[]]))
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), pi), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), pi), occupancy, settings)
 
     exp_dist = 2
     assert scan.shape[0] == lidar_n_rays

tests/lidar_sensor_pedestrian_test.py

@@ -27,7 +27,7 @@ def test_scanner_detects_single_pedestrian():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: np.array([[]]), lambda: pedestrians)
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), pi), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), pi), occupancy, settings)
 
     exp_dist = 2
     assert scan.shape[0] == lidar_n_rays
@@ -44,7 +44,7 @@ def test_scanner_detects_multiple_equidist_pedestrians_from_center():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: np.array([[]]), lambda: ped_pos)
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), 0), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), 0), occupancy, settings)
 
     exp_dist = 2.0
     assert scan.shape[0] == lidar_n_rays
@@ -57,7 +57,7 @@ def test_scanner_detects_only_closest_pedestrian():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: np.array([[]]), lambda: pedestrians)
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), pi), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), pi), occupancy, settings)
 
     exp_dist = 2
     assert scan.shape[0] == lidar_n_rays
@@ -69,7 +69,7 @@ def test_scanner_detects_nothing_when_there_is_nothing():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: np.array([[]]), lambda: np.array([[]]))
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), pi), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), pi), occupancy, settings)
 
     exp_dist = 5
     assert scan.shape[0] == lidar_n_rays
@@ -82,7 +82,7 @@ def test_scanner_detects_nothing_when_ray_pointing_to_other_side():
     occupancy = ContinuousOccupancy(10, 10, lambda: None, lambda: None, lambda: np.array([[]]), lambda: pedestrians)
     settings = LidarScannerSettings(5, 1, lidar_n_rays, scan_noise=NO_SCAN_NOISE)
 
-    scan = lidar_ray_scan(((0, 0), 0), occupancy, settings)
+    scan, _ = lidar_ray_scan(((0, 0), 0), occupancy, settings)
 
     exp_dist = 5
     assert scan.shape[0] == lidar_n_rays