Add path execution validation and abort (#211)

docs/source/usage/index.rst

@@ -1,6 +1,6 @@
 Usage
 =====
-This package works with or without ROS 2, and with or without a PyQt5 based GUI for visualization.
+This package works with or without ROS 2, and with or without a PySide6 based GUI for visualization.
 
 Refer to the following pages for different types of usage guides.
 

pyrobosim/examples/demo.py

@@ -102,7 +102,10 @@ def create_world(multirobot=False):
         name="robot0",
         radius=0.1,
         path_executor=ConstantVelocityExecutor(
-            linear_velocity=1.0, dt=0.1, max_angular_velocity=4.0
+            linear_velocity=1.0,
+            dt=0.1,
+            max_angular_velocity=4.0,
+            validate_during_execution=True,
         ),
         grasp_generator=GraspGenerator(grasp_props),
         partial_observability=args.partial_observability,

pyrobosim/pyrobosim/core/gazebo.py

@@ -11,7 +11,7 @@
 
 FOUR_SPACES = " " * 4
 EIGHT_SPACES = " " * 8
-TWELVE_SPACES = " " * 8
+TWELVE_SPACES = " " * 12
 
 
 class WorldGazeboExporter:
@@ -43,7 +43,7 @@ def export(self, classic=False, out_folder=None):
         Instructions to add to the Gazebo model path and spawn the world
         are printed to the Terminal.
 
-        :param classic: If True, exports to Gazebo Classic, else to Gazebo.
+        :param classic: If True, exports to Gazebo Classic, else to Gazebo Sim.
         :type classic: bool, optional
         :param out_folder: The output folder. If not specified, defaults to the pyrobosim `data/worlds` folder.
         :type out_folder: bool, optional

pyrobosim/pyrobosim/core/robot.py

@@ -586,7 +586,7 @@ def close_location(self):
             return False
 
         if isinstance(self.location, Hallway):
-            return self.world.close_hallway(self.location)
+            return self.world.close_hallway(self.location, ignore_robots=[self])
 
         # This should not happen
         return False

pyrobosim/pyrobosim/core/world.py

@@ -311,12 +311,14 @@ def open_hallway(self, hallway):
             self.gui.canvas.draw_and_sleep()
         return True
 
-    def close_hallway(self, hallway):
+    def close_hallway(self, hallway, ignore_robots=[]):
         """
         Close a hallway between two rooms.
 
         :param hallway: Hallway object to close.
         :type hallway: :class:`pyrobosim.core.hallway.Hallway`
+        :param ignore_robots: List of robots to ignore, for example the robot closing the hallway.
+        :type ignore_robots: list[:class:`pyrobosim.core.robot.Robot`]
         :return: True if the hallway was successfully closed, else False.
         :rtype: bool
         """
@@ -333,6 +335,11 @@ def close_hallway(self, hallway):
             warnings.warn(f"{hallway} is locked.")
             return False
 
+        for robot in [r for r in self.robots if r not in ignore_robots]:
+            if hallway.is_collision_free(robot.get_pose()):
+                warnings.warn(f"Robot {robot.name} is in {hallway}. Cannot close.")
+                return False
+
         hallway.is_open = False
         if self.has_gui:
             self.gui.canvas.show_hallways()
@@ -989,7 +996,7 @@ def get_hallways_from_rooms(self, room1, room2):
         for hall in room1.hallways:
             is_valid_hallway = (
                 (hall.room_start == room1) and (hall.room_end == room2)
-            ) or ((hall.room_end == room2) and (hall.room_end == room1))
+            ) or ((hall.room_start == room2) and (hall.room_end == room1))
             if is_valid_hallway:
                 hallways.append(hall)
         return hallways

pyrobosim/pyrobosim/core/yaml_utils.py

@@ -171,14 +171,11 @@ def get_path_executor(self, robot_data):
         if "path_executor" not in robot_data:
             return ConstantVelocityExecutor()
 
-        path_executor_data = robot_data["path_executor"]
+        path_executor_data = robot_data["path_executor"].copy()
         path_executor_type = path_executor_data["type"]
+        del path_executor_data["type"]
         if path_executor_type == "constant_velocity":
-            return ConstantVelocityExecutor(
-                linear_velocity=path_executor_data.get("linear_velocity", 1.0),
-                dt=path_executor_data.get("dt", 0.1),
-                max_angular_velocity=path_executor_data.get("max_angular_velocity"),
-            )
+            return ConstantVelocityExecutor(**path_executor_data)
         else:
             warnings.warn(f"Invalid path executor type specified: {path_executor_type}")
             return None
@@ -193,16 +190,11 @@ def get_grasp_generator(self, robot_data):
         if "grasping" not in robot_data:
             return None
 
-        grasp_params = robot_data["grasping"]
+        grasp_params = robot_data["grasping"].copy()
         grasp_gen_type = grasp_params["generator"]
+        del grasp_params["generator"]
         if grasp_gen_type == "parallel_grasp":
-            grasp_properties = ParallelGraspProperties(
-                max_width=grasp_params.get("max_width", 0.15),
-                depth=grasp_params.get("depth", 0.1),
-                height=grasp_params.get("height", 0.04),
-                width_clearance=grasp_params.get("width_clearance", 0.01),
-                depth_clearance=grasp_params.get("depth_clearance", 0.01),
-            )
+            grasp_properties = ParallelGraspProperties(**grasp_params)
             return GraspGenerator(grasp_properties)
         else:
             warnings.warn(f"Invalid grasp generator type specified: {grasp_gen_type}")

pyrobosim/pyrobosim/data/pddlstream_simple_world.yaml

@@ -34,6 +34,9 @@ robots:
       linear_velocity: 1.0
       max_angular_velocity: 4.0
       dt: 0.1
+      validate_during_execution: true
+      validation_dt: 0.5
+      validation_step_dist: 0.025
     grasping:
       generator: parallel_grasp
       max_width: 0.15

pyrobosim/pyrobosim/data/test_world.yaml

@@ -43,6 +43,10 @@ robots:
       linear_velocity: 1.0
       max_angular_velocity: 4.0
       dt: 0.1
+      validate_during_execution: true
+      validation_dt: 0.5
+      validation_step_dist: 0.025
+
     # Grasp generation
     grasping:
       generator: parallel_grasp

pyrobosim/pyrobosim/gui/__init__.py

@@ -1,6 +1,6 @@
 """ GUI utilities.
 
-This module contains the main pyrobosim UI, which is based on PyQt5,
+This module contains the main pyrobosim UI, which is based on PySide6,
 as well as the tools to embed the world model as a matplotlib canvas.
 
 The GUI allows users to view the state of the robot(s) in the world

pyrobosim/pyrobosim/navigation/execution.py

@@ -1,8 +1,10 @@
 """ Path execution utilities. """
 
 import time
+import threading
 import warnings
 
+from ..utils.motion import Path
 from ..utils.trajectory import get_constant_speed_trajectory, interpolate_trajectory
 
 
@@ -13,21 +15,44 @@ class ConstantVelocityExecutor:
     go to the next pose.
     """
 
-    def __init__(self, linear_velocity=1.0, dt=0.1, max_angular_velocity=None):
+    def __init__(
+        self,
+        dt=0.1,
+        linear_velocity=1.0,
+        max_angular_velocity=None,
+        validate_during_execution=False,
+        validation_dt=0.5,
+        validation_step_dist=0.025,
+    ):
         """
         Creates a constant velocity path executor.
 
-        :param linear_velocity: Linear velocity, in m/s, defaults to 1.0.
-        :type linear_velocity: float
-        :param dt: Time step for creating a trajectory, defaults to 0.1.
+        :param dt: Time step for creating a trajectory, in seconds.
         :type dt: float
+        :param linear_velocity: Linear velocity, in m/s.
+        :type linear_velocity: float
         :param max_angular_velocity: Maximum angular velocity, in rad/s.
         :type max_angular_velocity: float, optional
+        :param validate_during_execution: If True, runs a separate thread that validates the remaining path at a regular rate.
+        :type validate_during_execution: bool
+        :param validation_dt: Time step for validating the remaining path, in seconds.
+        :type validation_dt: float
+        :param validation_step_dist: The step size for discretizing a straight line to check collisions.
+        :type validation_step_dist: float
         """
+        self.robot = None
+        self.dt = dt
         self.linear_velocity = linear_velocity
         self.max_angular_velocity = max_angular_velocity
-        self.dt = dt
-        self.robot = None
+
+        self.validation_timer = None
+        self.validate_during_execution = validate_during_execution
+        self.validation_dt = validation_dt
+        self.validation_step_dist = validation_step_dist
+
+        # Execution state
+        self.current_traj_time = 0.0
+        self.abort_execution = False
 
     def execute(self, path, realtime_factor=1.0):
         """
@@ -47,31 +72,84 @@ def execute(self, path, realtime_factor=1.0):
         elif path.num_poses < 2:
             warnings.warn("Not enough waypoints in path to execute.")
             return False
+
         self.robot.executing_nav = True
+        self.current_traj_time = 0.0
+        self.abort_execution = False
 
-        # Convert the path to an interpolated trajectory
-        traj = get_constant_speed_trajectory(
+        # Convert the path to an interpolated trajectory.
+        self.traj = get_constant_speed_trajectory(
             path,
             linear_velocity=self.linear_velocity,
             max_angular_velocity=self.max_angular_velocity,
         )
-        traj_interp = interpolate_trajectory(traj, self.dt)
+        traj_interp = interpolate_trajectory(self.traj, self.dt)
+
+        # Optionally, kick off the path validation timer.
+        if self.validate_during_execution and self.robot.world is not None:
+            self.validation_timer = threading.Thread(
+                target=self.validate_remaining_path
+            )
+            self.validation_timer.start()
 
-        # Execute the trajectory
+        # Execute the trajectory.
+        success = True
         sleep_time = self.dt / realtime_factor
         is_holding_object = self.robot.manipulated_object is not None
         for i in range(traj_interp.num_points()):
             start_time = time.time()
             cur_pose = traj_interp.poses[i]
+            self.current_traj_time = traj_interp.t_pts[i]
             self.robot.set_pose(cur_pose)
             if is_holding_object:
                 self.robot.manipulated_object.set_pose(cur_pose)
+
+            if self.abort_execution:
+                warnings.warn("Trajectory execution aborted.")
+                success = False
+                break
+
             time.sleep(max(0, sleep_time - (time.time() - start_time)))
 
-        # Finalize path execution
+        # Finalize path execution.
         time.sleep(0.1)  # To ensure background threads get the end of the path.
+        self.abort_execution = True
+        if self.validate_during_execution:
+            self.validation_timer.join()
         self.robot.executing_nav = False
-        self.robot.last_nav_successful = True
+        self.robot.last_nav_successful = success
         self.robot.executing_action = False
         self.robot.current_action = None
         return self.robot.last_nav_successful
+
+    def validate_remaining_path(self):
+        """
+        Validates the remaining path by checking collisions against the world.
+
+        This function will set the `abort_execution` attribute to `True`,
+        which cancels the main trajectory execution loop.
+        """
+        while self.robot.executing_nav and not self.abort_execution:
+            start_time = time.time()
+            cur_pose = self.robot.get_pose()
+            cur_time = self.current_traj_time
+
+            # Get the waypoint index of the remaining path.
+            for idx, t in enumerate(self.traj.t_pts):
+                if t >= cur_time:
+                    break
+            if idx == self.traj.num_points() - 1:
+                return
+
+            # Collision check the remaining path.
+            poses = [cur_pose]
+            poses.extend(self.traj.poses[idx:])
+            if len(poses) > 2:
+                remaining_path = Path(poses=poses)
+                if not remaining_path.is_collision_free(
+                    self.robot.world, step_dist=self.validation_step_dist
+                ):
+                    warnings.warn("Remaining path is in collision. Aborting execution.")
+                    self.abort_execution = True
+
+            time.sleep(max(0, self.validation_dt - (time.time() - start_time)))

pyrobosim/pyrobosim/utils/motion.py

@@ -43,6 +43,24 @@ def fill_yaws(self):
             yaw = prev_pose.get_angular_distance(cur_pose)
             cur_pose.set_euler_angles(yaw=yaw)
 
+    def is_collision_free(self, world, step_dist=0.01):
+        """
+        Check whether a path is collision free in a specific world.
+
+        :param world: The world to use for collision checking.
+        :type world: :class:`pyrobosim.core.world.World`
+        :param step_dist: The step size for discretizing a straight line to check collisions.
+        :type step_dist: float
+        :return: True if the path is collision free, else False.
+        :rtype: bool
+        """
+        for idx in range(len(self.poses) - 1):
+            if not world.is_connectable(
+                self.poses[idx], self.poses[idx + 1], step_dist=step_dist
+            ):
+                return False
+        return True
+
     def __eq__(self, other):
         """
         Check if two paths are exactly equal.
@@ -53,7 +71,7 @@ def __eq__(self, other):
         :rtype: bool
         """
         if not (isinstance(other, Path)):
-            raise TypeError("Expected a Path object")
+            raise TypeError("Expected a Path object.")
 
         return self.poses == other.poses
 

pyrobosim/pyrobosim/utils/pose.py

@@ -208,7 +208,7 @@ def is_approx(self, other, rel_tol=1e-06, abs_tol=1e-06):
         :rtype: bool
         """
         if not (isinstance(other, Pose)):
-            raise TypeError("Expected a Pose")
+            raise TypeError("Expected a Pose object.")
 
         return np.allclose(
             self.get_translation(), other.get_translation(), rel_tol, abs_tol
@@ -224,7 +224,7 @@ def __eq__(self, other):
         :rtype: bool
         """
         if not (isinstance(other, Pose)):
-            raise TypeError("Expected a Pose object")
+            raise TypeError("Expected a Pose object.")
 
         return np.all(self.get_translation() == other.get_translation()) and np.all(
             self.q == other.q

pyrobosim/pyrobosim/utils/trajectory.py

@@ -126,6 +126,9 @@ def interpolate_trajectory(traj: Trajectory, dt: float):
         warnings.warn("Insufficient trajectory points for interpolation.")
         return None
 
+    if traj.num_points() == 2 and traj.poses[0] == traj.poses[1]:
+        return traj
+
     # De-duplicate time points ensure that Slerp doesn't throw an error.
     # Right now, we're just keeping the later point.
     i = 1