Add Reset Path Planner UI button (#249)

pyrobosim/pyrobosim/core/robot.py

@@ -421,6 +421,26 @@ def navigate(
 
         return self.follow_path(path, realtime_factor=realtime_factor)
 
+    def reset_path_planner(self):
+        """Resets the robot's path planner, if available."""
+        if self.path_planner is None:
+            warnings.warn(f"[{self.name}] Robot has no path planner. Cannot reset.")
+            return
+
+        if not hasattr(self.path_planner, "reset"):
+            warnings.warn(
+                f"[{self.name}] Path planner does not have a reset() method. Cannot reset."
+            )
+            return
+
+        self.path_planner.reset()
+        if self.world.has_gui:
+            show_graphs = True
+            path = None
+            self.world.gui.canvas.show_planner_and_path_signal.emit(
+                self, show_graphs, path
+            )
+
     def pick_object(self, obj_query, grasp_pose=None):
         """
         Picks up an object in the world given an object and/or location query.

pyrobosim/pyrobosim/core/yaml_utils.py

@@ -7,11 +7,7 @@
 
 from .robot import Robot
 from .world import World
-from ..navigation import (
-    ConstantVelocityExecutor,
-    OccupancyGrid,
-    get_planner_class,
-)
+from ..navigation import ConstantVelocityExecutor, get_planner_class
 from ..planning.actions import ExecutionOptions
 from ..utils.general import replace_special_yaml_tokens
 from ..utils.pose import Pose

pyrobosim/pyrobosim/gui/main.py

@@ -169,9 +169,12 @@ def create_layout(self):
         self.other_options_layout.addWidget(
             self.toggle_collision_polygons_checkbox, 0, 0
         )
+        self.reset_path_planner_button = QtWidgets.QPushButton("Reset path planner")
+        self.reset_path_planner_button.clicked.connect(self.on_reset_path_planner_click)
+        self.other_options_layout.addWidget(self.reset_path_planner_button, 0, 1)
         self.cancel_action_button = QtWidgets.QPushButton("Cancel action")
         self.cancel_action_button.clicked.connect(self.on_cancel_action_click)
-        self.other_options_layout.addWidget(self.cancel_action_button, 0, 1)
+        self.other_options_layout.addWidget(self.cancel_action_button, 0, 2)
 
         # Main layout
         self.main_layout = QtWidgets.QVBoxLayout(self.main_widget)
@@ -196,22 +199,25 @@ def update_button_state(self):
         """Update the state of buttons based on the state of the robot."""
         robot = self.get_current_robot()
         if robot:
+            is_moving = robot.is_moving()
             at_open_object_spawn = robot.at_object_spawn() and robot.location.is_open
             can_pick = robot.manipulated_object is None
             can_open_close = robot.at_openable_location() and can_pick
 
-            self.nav_button.setEnabled(not robot.is_moving())
+            self.nav_button.setEnabled(not is_moving)
             self.pick_button.setEnabled(can_pick and at_open_object_spawn)
             self.place_button.setEnabled((not can_pick) and at_open_object_spawn)
             self.detect_button.setEnabled(at_open_object_spawn)
             self.open_button.setEnabled(can_open_close and not robot.location.is_open)
             self.close_button.setEnabled(can_open_close and robot.location.is_open)
-            self.cancel_action_button.setEnabled(robot.is_moving())
+            self.cancel_action_button.setEnabled(is_moving)
+            self.reset_path_planner_button.setEnabled(not is_moving)
 
-            self.canvas.show_world_state(robot, navigating=False)
+            self.canvas.show_world_state(robot, navigating=is_moving)
         else:
             self.nav_button.setEnabled(False)
             self.cancel_action_button.setEnabled(False)
+            self.reset_path_planner_button.setEnabled(False)
 
         self.canvas.draw_signal.emit()
 
@@ -231,6 +237,7 @@ def set_buttons_during_action(self, state):
         self.close_button.setEnabled(state)
         self.rand_pose_button.setEnabled(state)
         self.cancel_action_button.setEnabled(not state)
+        self.reset_path_planner_button.setEnabled(state)
 
     ####################
     # Button Callbacks #
@@ -337,3 +344,10 @@ def on_cancel_action_click(self):
         if robot:
             robot.cancel_actions()
             self.canvas.draw_signal.emit()
+
+    def on_reset_path_planner_click(self):
+        """Callback to reset the path planner for the current robot."""
+        robot = self.get_current_robot()
+        if robot:
+            robot.reset_path_planner()
+            self.canvas.draw_signal.emit()

pyrobosim/test/core/test_robot.py

@@ -122,10 +122,8 @@ def test_robot_path_planner(self):
         init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
         goal_pose = Pose(x=2.5, y=3.0, yaw=np.pi / 2.0)
 
-        robot = Robot(
-            pose=init_pose,
-            path_planner=WorldGraphPlanner(world=self.test_world),
-        )
+        path_planner = WorldGraphPlanner(world=self.test_world)
+        robot = Robot(pose=init_pose, path_planner=path_planner)
         robot.world = self.test_world
         robot.location = self.test_world.get_entity_by_name("kitchen")
 
@@ -156,6 +154,18 @@ def test_robot_path_planner(self):
             warn_info[0].message.args[0] == "No path planner attached to robot robot."
         )
 
+        # Try and reset the path planner with no planner set
+        with pytest.warns(UserWarning) as warn_info:
+            robot.reset_path_planner()
+        assert (
+            warn_info[0].message.args[0]
+            == "[robot] Robot has no path planner. Cannot reset."
+        )
+
+        # Re-add the path planner and reset it. There should be no warnings.
+        robot.path_planner = path_planner
+        robot.reset_path_planner()
+
     def test_robot_path_executor(self):
         """Check that path executors can be used from a robot."""
         init_pose = Pose(x=1.0, y=0.5, yaw=0.0)

pyrobosim_ros/pyrobosim_ros/ros_interface.py

@@ -571,13 +571,7 @@ def robot_path_planner_reset_callback(self, request, response, robot=None):
             self.get_logger().warn(message)
             return Trigger.Response(success=False, message=message)
 
-        robot.path_planner.reset()
-        if self.world.has_gui:
-            show_graphs = True
-            path = None
-            self.world.gui.canvas.show_planner_and_path_signal.emit(
-                robot, show_graphs, path
-            )
+        robot.reset_path_planner()
         return Trigger.Response(success=True)
 
     def robot_detect_objects_callback(self, goal_handle, robot=None):