Add UI button to toggle collision polygon display (#242)

pyrobosim/pyrobosim/gui/main.py

@@ -157,6 +157,13 @@ def create_layout(self):
         self.nav_toolbar = NavigationToolbar2QT(self.canvas, self)
         self.addToolBar(QtCore.Qt.BottomToolBarArea, self.nav_toolbar)
         self.world_layout.addWidget(self.canvas)
+        self.toggle_collision_polygons_checkbox = QtWidgets.QCheckBox(
+            "Show collision polygons"
+        )
+        self.toggle_collision_polygons_checkbox.clicked.connect(
+            self.on_collision_polygon_toggle_click
+        )
+        self.world_layout.addWidget(self.toggle_collision_polygons_checkbox)
 
         # Main layout
         self.main_layout = QtWidgets.QVBoxLayout(self.main_widget)
@@ -306,3 +313,8 @@ def on_close_click(self):
             print(f"[{robot.name}] Closing {robot.location}")
             self.canvas.close_location(robot)
             self.update_button_state()
+
+    def on_collision_polygon_toggle_click(self):
+        """Callback to toggle collision polygons."""
+        self.canvas.toggle_collision_polygons()
+        self.canvas.draw_signal.emit()

pyrobosim/pyrobosim/gui/world_canvas.py

@@ -145,11 +145,10 @@ def __init__(
         self.obj_patches = []
         self.obj_texts = []
         self.hallway_patches = []
+        self.room_patches = []
         self.location_patches = []
         self.location_texts = []
         self.path_planner_artists = {"graph": [], "path": []}
-
-        # Debug displays (TODO: Should be available from GUI).
         self.show_collision_polygons = False
 
         # Connect signals
@@ -171,6 +170,17 @@ def __init__(
             self.nav_animator.timeout.connect(self.nav_animation_callback)
             self.nav_animator.start(sleep_time_msec)
 
+    def toggle_collision_polygons(self):
+        """Shows/hides collision polygons."""
+        self.show_collision_polygons = not self.show_collision_polygons
+        print(
+            "Enabling collision polygons"
+            if self.show_collision_polygons
+            else "Disabling collision polygons"
+        )
+        self.show_hallways()
+        self.show_rooms()
+
     def show_robots(self):
         """Draws robots as circles with heading lines for visualization."""
         with self.draw_lock:
@@ -239,6 +249,31 @@ def show_hallways(self):
                     self.axes.add_patch(coll_patch)
                     self.hallway_patches.append(coll_patch)
 
+    def show_rooms(self):
+        """Draws rooms in the world."""
+        with self.draw_lock:
+            for room in self.room_patches:
+                room.remove()
+
+            self.room_patches = [room.viz_patch for room in self.world.rooms]
+
+            for room in self.world.rooms:
+                self.axes.add_patch(room.viz_patch)
+                self.axes.text(
+                    room.centroid[0],
+                    room.centroid[1],
+                    room.name,
+                    color=room.viz_color,
+                    fontsize=12,
+                    ha="center",
+                    va="top",
+                    clip_on=True,
+                )
+                if self.show_collision_polygons:
+                    coll_patch = room.get_collision_patch()
+                    self.axes.add_patch(coll_patch)
+                    self.room_patches.append(coll_patch)
+
     def show_locations(self):
         """Draws locations and object spawns in the world."""
         with self.draw_lock:
@@ -307,21 +342,7 @@ def show(self):
         Displays all entities in the world (robots, rooms, objects, etc.).
         """
         # Entities in the world.
-        self.room_patches = [room.viz_patch for room in self.world.rooms]
-        for room in self.world.rooms:
-            self.axes.add_patch(room.viz_patch)
-            self.axes.text(
-                room.centroid[0],
-                room.centroid[1],
-                room.name,
-                color=room.viz_color,
-                fontsize=12,
-                ha="center",
-                va="top",
-                clip_on=True,
-            )
-            if self.show_collision_polygons:
-                self.axes.add_patch(room.get_collision_patch())
+        self.show_rooms()
         self.show_hallways()
         self.show_locations()
         self.show_objects()