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 @@
+
+
+
+
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:
diff --git a/robot_sf/render/sim_view.py b/robot_sf/render/sim_view.py
index 0a50477..0f1d8cb 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)
@@ -133,15 +135,24 @@ def _handle_video_resize(self, e):
def _handle_keydown(self, e):
"""Handle key presses for the simulation view."""
+ new_offset = 100
+ new_scaling = 1
+ if pygame.key.get_mods() & pygame.KMOD_CTRL:
+ 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] - 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)),
# focus on the robot
@@ -211,6 +222,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()
@@ -275,6 +290,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(
@@ -352,6 +389,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}'