diff --git a/examples/demo_defensive.py b/examples/demo_defensive.py index aeb0169..0886211 100644 --- a/examples/demo_defensive.py +++ b/examples/demo_defensive.py @@ -50,15 +50,15 @@ def obs_adapter(orig_obs): ray_state = np.squeeze(ray_state) return np.concatenate((ray_state, drive_state), axis=0) - obs = env.reset() + obs, _ = env.reset() for _ in range(10000): obs = obs_adapter(obs) action, _ = model.predict(obs, deterministic=True) - obs, _, done, _ = env.step(action) + obs, _, done, _, _ = env.step(action) env.render() if done: - obs = env.reset() + obs, _ = env.reset() env.render() env.exit() diff --git a/examples/demo_offensive.py b/examples/demo_offensive.py index 55caa4b..7fe3556 100644 --- a/examples/demo_offensive.py +++ b/examples/demo_offensive.py @@ -9,17 +9,17 @@ def training(): env_config = EnvSettings( 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, recording_enabled=True) + env = RobotEnv(env_config, debug=True, recording_enabled=False) model = PPO.load("./model/run_043", env=env) - obs = env.reset() + obs, _ = env.reset() for _ in range(10000): action, _ = model.predict(obs, deterministic=True) - obs, _, done, _ = env.step(action) + obs, _, done, _, _ = env.step(action) env.render() if done: - obs = env.reset() + obs, _ = env.reset() env.render() env.exit() diff --git a/examples/ego_ped_example.py b/examples/ego_ped_example.py index 327a342..526bebe 100644 --- a/examples/ego_ped_example.py +++ b/examples/ego_ped_example.py @@ -30,7 +30,7 @@ def test_simulation(map_definition: MapDefinition): env = PedestrianEnv(env_config, robot_model=robot_model, debug=True, recording_enabled=True) - obs = env.reset() + obs, _ = env.reset() logger.info("Simulating the random policy.") for _ in range(10000): @@ -39,7 +39,7 @@ def test_simulation(map_definition: MapDefinition): env.render() if done: - obs = env.reset() + obs, _ = env.reset() env.render() env.reset() @@ -56,7 +56,7 @@ def get_file(): def main(): - map_def = convert_map("maps/svg_maps/debug_03.svg") + map_def = convert_map("maps/svg_maps/debug_06.svg") test_simulation(map_def) diff --git a/examples/simulate_with_svg_map.py b/examples/simulate_with_svg_map.py index f1aefd9..232dbd8 100644 --- a/examples/simulate_with_svg_map.py +++ b/examples/simulate_with_svg_map.py @@ -40,16 +40,16 @@ def obs_adapter(orig_obs): ray_state = np.squeeze(ray_state) return np.concatenate((ray_state, drive_state), axis=0) - obs = env.reset() + obs, _ = env.reset() logger.info("Simulating the random policy.") for _ in range(10000): obs = obs_adapter(obs) action = env.action_space.sample() - obs, _, done, _ = env.step(action) + obs, _, done, _, _ = env.step(action) env.render() if done: - obs = env.reset() + obs, _ = env.reset() env.render() env.exit() @@ -58,9 +58,7 @@ def main(): """Simulate a random policy with a map defined in SVG format.""" logger.info("Simulating a random policy with the map.") - # svg_file = "maps/svg_maps/02_simple_maps.svg" - # svg_file = "maps/svg_maps/03_mid_object.svg" - svg_file = "maps/svg_maps/04_small_mid_object.svg" + svg_file = "maps/svg_maps/debug_06.svg" logger.info("Converting SVG map to MapDefinition object.") logger.info(f"SVG file: {svg_file}") diff --git a/examples/view_recording.py b/examples/view_recording.py index 37c94b2..5722a07 100644 --- a/examples/view_recording.py +++ b/examples/view_recording.py @@ -2,15 +2,12 @@ import os from pathlib import Path -import numpy as np from loguru import logger from robot_sf.nav.svg_map_parser import SvgMapConverter from robot_sf.nav.map_config import MapDefinition, MapDefinitionPool -from robot_sf.gym_env.env_config import SimulationSettings, EnvSettings +from robot_sf.gym_env.env_config import EnvSettings from robot_sf.gym_env.robot_env import RobotEnv -from robot_sf.sensor.sensor_fusion import OBS_RAYS, OBS_DRIVE_STATE -from robot_sf.robot.differential_drive import DifferentialDriveSettings from robot_sf.render.playback_recording import load_states_and_visualize diff --git a/robot_sf/gym_env/pedestrian_env.py b/robot_sf/gym_env/pedestrian_env.py index 5436719..bb378f1 100644 --- a/robot_sf/gym_env/pedestrian_env.py +++ b/robot_sf/gym_env/pedestrian_env.py @@ -205,6 +205,9 @@ def reset(self, seed=None, options=None): - obs: The initial observation after resetting the environment. """ super().reset(seed=seed, options=options) + # Reset last actions + self.last_action_robot = None + self.last_action_ped = None # Reset internal simulator state self.simulator.reset_state() # Reset the environment's state and return the initial observation diff --git a/robot_sf/render/sim_view.py b/robot_sf/render/sim_view.py index c363aee..66e5f23 100644 --- a/robot_sf/render/sim_view.py +++ b/robot_sf/render/sim_view.py @@ -65,7 +65,7 @@ class VisualizableSimState: ped_actions: np.ndarray ego_ped_pose: PedPose = None ego_ped_ray_vecs: np.ndarray = None - ego_ped_actions: Union[VisualizableAction, None] = None + ego_ped_action: Union[VisualizableAction, None] = None # obstacles: List[Obstacle] @@ -91,7 +91,7 @@ class SimulationView: focus_on_robot: bool = False focus_on_ego_ped: bool = False display_help: bool = False - display_robot_info: bool = False + display_robot_info: int = 0 """The offset is already uses `scaling` as a factor.""" @property @@ -172,7 +172,8 @@ def _handle_keydown(self, e): # display help pygame.K_h: lambda: setattr(self, 'display_help', not self.display_help), # display robotinfo - pygame.K_q: lambda: setattr(self, 'display_robot_info', not self.display_robot_info), + pygame.K_q: lambda: setattr(self, 'display_robot_info', + (self.display_robot_info + 1) % 3), } if e.key in key_action_map: @@ -204,7 +205,6 @@ def clear(self): adds text at position 0, and updates the display. """ self.screen.fill(BACKGROUND_COLOR) - self._augment_timestep(0) pygame.display.update() def render(self, state: VisualizableSimState): @@ -255,11 +255,11 @@ def render(self, state: VisualizableSimState): self._draw_robot(state.robot_pose) if state.ego_ped_pose: self._augment_lidar(state.ego_ped_ray_vecs) - self._augment_action(state.ego_ped_actions, EGO_PED_ACTION_COLOR) + if state.ego_ped_action: + self._augment_action(state.ego_ped_action, EGO_PED_ACTION_COLOR) self._draw_ego_ped(state.ego_ped_pose) # information - self._augment_timestep(state.timestep) self._add_text(state.timestep, state) if self.display_help: self._add_help_text() @@ -280,7 +280,7 @@ def _move_camera(self, state: VisualizableSimState): r_x, r_y = state.robot_pose[0] self.offset[0] = int(r_x * self.scaling - self.width / 2) * -1 self.offset[1] = int(r_y * self.scaling - self.height / 2) * -1 - if self.focus_on_ego_ped: + if self.focus_on_ego_ped and state.ego_ped_pose: r_x, r_y = state.ego_ped_pose[0] self.offset[0] = int(r_x * self.scaling - self.width / 2) * -1 self.offset[1] = int(r_y * self.scaling - self.height / 2) * -1 @@ -427,25 +427,23 @@ def _draw_coordinates(self, x, y): text = self.font.render(f'({x}, {y})', False, TEXT_COLOR) self.screen.blit(text, (x, y)) - def _augment_timestep(self, timestep: int): - # TODO: show map name as well - text = f'step: {timestep}' - text_surface = self.font.render(text, False, TEXT_COLOR) - self.screen.blit(text_surface, self._timestep_text_pos) - def _add_text(self, timestep: int, state: VisualizableSimState): - if self.display_robot_info: - lines = [ + lines = [] + if self.display_robot_info == 1 and state.robot_action: + lines += [ f'RobotPose: {state.robot_pose}', - f'RobotAction: {state.robot_action.action}', - f'RobotGoal: {state.robot_action.goal}'] - else: - distance_to_robot = euclid_dist(state.ego_ped_pose[0], state.robot_pose[0]) - lines = [ - f'PedestrianPose: {state.ego_ped_pose}', - f'PedestrianAction: {state.ego_ped_actions.action}', - f'PedestrianGoal: {state.ego_ped_actions.goal}', - f'DistanceRobot: {distance_to_robot:.2f}',] + f'RobotAction: {state.robot_action.action if state.robot_action else None}', + f'RobotGoal: {state.robot_action.goal if state.robot_action else None}',] + elif self.display_robot_info == 2: + if state.ego_ped_pose and state.ego_ped_action: + distance_to_robot = euclid_dist(state.ego_ped_pose[0], state.robot_pose[0]) + lines += [ + f'PedestrianPose: {state.ego_ped_pose}', + f'PedestrianAction: {state.ego_ped_action.action}', + f'PedestrianGoal: {state.ego_ped_action.goal}', + f'DistanceRobot: {distance_to_robot:.2f}',] + else: + self.display_robot_info = 0 text_lines = [ f'step: {timestep}', f'scaling: {self.scaling}',