From 1b96e87cf2c737c796e5140c052a9e1f8ce50e4e Mon Sep 17 00:00:00 2001 From: Julius Miller Date: Tue, 2 Jul 2024 13:20:43 +0200 Subject: [PATCH 1/4] feat: add ctrl_key for faster sim_view + change inverted sim_view controls --- robot_sf/render/sim_view.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/robot_sf/render/sim_view.py b/robot_sf/render/sim_view.py index 5ea957e..4846f58 100644 --- a/robot_sf/render/sim_view.py +++ b/robot_sf/render/sim_view.py @@ -164,15 +164,19 @@ def _handle_video_resize(self, e): def _handle_keydown(self, e): """Handle key presses for the simulation view.""" + new_offset = 10 + if pygame.key.get_mods() & pygame.KMOD_CTRL: + new_offset = 100 + key_action_map = { # scale the view pygame.K_PLUS: lambda: setattr(self, 'scaling', self.scaling + 1), pygame.K_MINUS: lambda: setattr(self, 'scaling', max(self.scaling - 1, 1)), # move the view - pygame.K_LEFT: lambda: self.offset.__setitem__(0, self.offset[0] - 10), - pygame.K_RIGHT: lambda: self.offset.__setitem__(0, self.offset[0] + 10), - pygame.K_UP: lambda: self.offset.__setitem__(1, self.offset[1] - 10), - pygame.K_DOWN: lambda: self.offset.__setitem__(1, self.offset[1] + 10), + pygame.K_LEFT: lambda: self.offset.__setitem__(0, self.offset[0] + new_offset), + pygame.K_RIGHT: lambda: self.offset.__setitem__(0, self.offset[0] - new_offset), + pygame.K_UP: lambda: self.offset.__setitem__(1, self.offset[1] + new_offset), + pygame.K_DOWN: lambda: self.offset.__setitem__(1, self.offset[1] - new_offset), # reset the view pygame.K_r: lambda: self.offset.__setitem__(slice(None), (0, 0)), } From 29d59bb9cf1ded43baa148389e5eed78a9251c94 Mon Sep 17 00:00:00 2001 From: Julius Miller Date: Tue, 9 Jul 2024 16:07:31 +0200 Subject: [PATCH 2/4] feat: draw spawn/goal zones + update controls for viewing --- robot_sf/render/sim_view.py | 48 +++++++++++++++++++++++++++++++++---- 1 file changed, 44 insertions(+), 4 deletions(-) diff --git a/robot_sf/render/sim_view.py b/robot_sf/render/sim_view.py index 4846f58..2f9356c 100644 --- a/robot_sf/render/sim_view.py +++ b/robot_sf/render/sim_view.py @@ -27,6 +27,8 @@ BACKGROUND_COLOR = (255, 255, 255) BACKGROUND_COLOR_TRANSP = (255, 255, 255, 128) OBSTACLE_COLOR = (20, 30, 20, 128) +PED_SPAWN_COLOR = (255, 204, 203) +PED_GOAL_COLOR = (144, 238, 144) PED_COLOR = (255, 50, 50) PED_ROUTE_COLOR = (0, 0, 255) ROBOT_ROUTE_COLOR = (30, 30, 255) @@ -164,14 +166,19 @@ def _handle_video_resize(self, e): def _handle_keydown(self, e): """Handle key presses for the simulation view.""" - new_offset = 10 + new_offset = 100 + new_scaling = 1 if pygame.key.get_mods() & pygame.KMOD_CTRL: - new_offset = 100 + new_offset = 250 + new_scaling = 10 + + if pygame.key.get_mods() & pygame.KMOD_ALT: + new_offset = 10 key_action_map = { # scale the view - pygame.K_PLUS: lambda: setattr(self, 'scaling', self.scaling + 1), - pygame.K_MINUS: lambda: setattr(self, 'scaling', max(self.scaling - 1, 1)), + pygame.K_PLUS: lambda: setattr(self, 'scaling', self.scaling + new_scaling), + pygame.K_MINUS: lambda: setattr(self, 'scaling', max(self.scaling - new_scaling, 1)), # move the view pygame.K_LEFT: lambda: self.offset.__setitem__(0, self.offset[0] + new_offset), pygame.K_RIGHT: lambda: self.offset.__setitem__(0, self.offset[0] - new_offset), @@ -243,6 +250,10 @@ def render(self, state: VisualizableSimState): self._draw_robot_routes() if self.map_def.ped_routes: self._draw_pedestrian_routes() + if self.map_def.ped_spawn_zones: + self._draw_spawn_zones() + if self.map_def.ped_goal_zones: + self._draw_goal_zones() self._draw_grid() @@ -301,6 +312,28 @@ def _draw_obstacles(self): # Draw the obstacle as a polygon on the screen pygame.draw.polygon(self.screen, OBSTACLE_COLOR, scaled_vertices) + def _draw_spawn_zones(self): + # Iterate over each spawn_zone in the list of spawn_zones + for spawn_zone in self.map_def.ped_spawn_zones: + # Scale and offset the vertices of the zones + vertices_np = np.array(spawn_zone) + scaled_vertices = [( + self._scale_tuple((x, y)) + ) for x, y in vertices_np] + # Draw the spawn zone as a polygon on the screen + pygame.draw.polygon(self.screen, PED_SPAWN_COLOR, scaled_vertices) + + def _draw_goal_zones(self): + # Iterate over each goal_zone in the list of goal_zones + for goal_zone in self.map_def.ped_goal_zones: + # Scale and offset the vertices of the goal zones + vertices_np = np.array(goal_zone) + scaled_vertices = [( + self._scale_tuple((x, y)) + ) for x, y in vertices_np] + # Draw the goal_zone as a polygon on the screen + pygame.draw.polygon(self.screen, PED_GOAL_COLOR, scaled_vertices) + def _augment_goal_position(self, robot_goal: Vec2D): # TODO: display pedestrians with an image instead of a circle pygame.draw.circle( @@ -378,6 +411,13 @@ def _draw_robot_routes(self): width = 1 ) + def _draw_coordinates(self, x, y): + """ + Draws the coordinates (x, y) on the screen. + """ + 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}' From e74576be7aa4e1d48277f7292064ebf7340d4fb8 Mon Sep 17 00:00:00 2001 From: Julius Miller Date: Tue, 9 Jul 2024 16:08:45 +0200 Subject: [PATCH 3/4] feat: Add new maps --- examples/simulate_with_svg_map.py | 4 +- maps/svg_maps/03_mid_object.svg | 129 ++++++++++++++++++++++++++ maps/svg_maps/04_small_mid_object.svg | 129 ++++++++++++++++++++++++++ 3 files changed, 261 insertions(+), 1 deletion(-) create mode 100644 maps/svg_maps/03_mid_object.svg create mode 100644 maps/svg_maps/04_small_mid_object.svg diff --git a/examples/simulate_with_svg_map.py b/examples/simulate_with_svg_map.py index 956344e..947d645 100644 --- a/examples/simulate_with_svg_map.py +++ b/examples/simulate_with_svg_map.py @@ -58,7 +58,9 @@ 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/02_simple_maps.svg" + #svg_file = "maps/svg_maps/03_mid_object.svg" + svg_file = "maps/svg_maps/04_small_mid_object.svg" logger.info("Converting SVG map to MapDefinition object.") logger.info(f"SVG file: {svg_file}") diff --git a/maps/svg_maps/03_mid_object.svg b/maps/svg_maps/03_mid_object.svg new file mode 100644 index 0000000..7d4ee70 --- /dev/null +++ b/maps/svg_maps/03_mid_object.svg @@ -0,0 +1,129 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/maps/svg_maps/04_small_mid_object.svg b/maps/svg_maps/04_small_mid_object.svg new file mode 100644 index 0000000..ee04e78 --- /dev/null +++ b/maps/svg_maps/04_small_mid_object.svg @@ -0,0 +1,129 @@ + + + + + + + + + + + + + + + + + + + + From ea64bd25fab0ff5fa2de2abb2c478f12c6c1be9c Mon Sep 17 00:00:00 2001 From: Julius Miller Date: Tue, 9 Jul 2024 16:14:08 +0200 Subject: [PATCH 4/4] feat: Option to use dedicated spawn/goal zones --- robot_sf/nav/svg_map_parser.py | 63 ++++++++++++++++++++-------------- 1 file changed, 37 insertions(+), 26 deletions(-) diff --git a/robot_sf/nav/svg_map_parser.py b/robot_sf/nav/svg_map_parser.py index c390aac..2103a80 100644 --- a/robot_sf/nav/svg_map_parser.py +++ b/robot_sf/nav/svg_map_parser.py @@ -47,6 +47,8 @@ def _get_svg_info(self): """ Extracts path and rectangle information from an SVG file. + It is important that the SVG file uses absolute coordinates for the paths. + This method finds all 'path' and 'rect' elements in the SVG file and extracts their coordinates, labels, and ids. The information is stored in the 'path_info' and 'rect_info' attributes of the SvgMapConverter instance. @@ -162,13 +164,33 @@ def _info_to_mapdefintion(self) -> MapDefinition: ped_crowded_zones: List[Rect] = [] ped_routes: List[GlobalRoute] = [] + for rect in self.rect_info: + if rect.label == 'robot_spawn_zone': + robot_spawn_zones.append(rect.get_zone()) + elif rect.label == 'ped_spawn_zone': + ped_spawn_zones.append(rect.get_zone()) + elif rect.label == 'robot_goal_zone': + robot_goal_zones.append(rect.get_zone()) + elif rect.label == 'bound': + bounds.append(rect.get_zone()) + elif rect.label == 'ped_goal_zone': + ped_goal_zones.append(rect.get_zone()) + elif rect.label == 'obstacle': + obstacles.append(obstacle_from_svgrectangle(rect)) + elif rect.label == 'ped_crowded_zone': + ped_crowded_zones.append(rect.get_zone()) + else: + logger.error( + f"Unknown label <{rect.label}> in id <{rect.id_}>" + ) + for path in self.path_info: # check the label of the path if path.label == 'obstacle': # Convert the coordinates to a list of vertices vertices = path.coordinates.tolist() - + # Check if the first and last vertices are the same if not np.array_equal(vertices[0], vertices[-1]): logger.warning( @@ -183,18 +205,27 @@ def _info_to_mapdefintion(self) -> MapDefinition: # Append the obstacle to the list obstacles.append(Obstacle(vertices)) - elif path.label == 'ped_route': + elif 'ped_route' in path.label: # Convert the coordinates to a list of vertices vertices = path.coordinates.tolist() + # ped_routes have a label of the form 'ped_route__' + numbers = re.findall(r'\d+', path.label) + if numbers: + spawn = int(numbers[0]) + goal = int(numbers[1]) + else: + spawn = 0 + goal = 0 + # Append the obstacle to the list ped_routes.append( GlobalRoute( - spawn_id=0, # TODO: What is this? value is arbitrary - goal_id=0, # TODO: What is this? value is arbitrary + spawn_id=spawn, + goal_id=goal, waypoints=vertices, - spawn_zone=(vertices[0], 0, 0), # TODO - goal_zone=(vertices[-1], 0, 0) # TODO + spawn_zone=ped_spawn_zones[spawn] if ped_spawn_zones else (vertices[0],0,0), + goal_zone=ped_goal_zones[goal] if ped_goal_zones else (vertices[-1],0,0) )) elif path.label == 'robot_route': @@ -225,26 +256,6 @@ def _info_to_mapdefintion(self) -> MapDefinition: f"Unknown label <{path.label}> in id <{path.id}>" ) - for rect in self.rect_info: - if rect.label == 'robot_spawn_zone': - robot_spawn_zones.append(rect.get_zone()) - elif rect.label == 'ped_spawn_zone': - ped_spawn_zones.append(rect.get_zone()) - elif rect.label == 'robot_goal_zone': - robot_goal_zones.append(rect.get_zone()) - elif rect.label == 'bound': - bounds.append(rect.get_zone()) - elif rect.label == 'ped_goal_zone': - ped_goal_zones.append(rect.get_zone()) - elif rect.label == 'obstacle': - obstacles.append(obstacle_from_svgrectangle(rect)) - elif rect.label == 'ped_crowded_zone': - ped_crowded_zones.append(rect.get_zone()) - else: - logger.error( - f"Unknown label <{rect.label}> in id <{rect.id_}>" - ) - if not obstacles: