Merge branch 'main' into hallways-hack

sea-bass · Aug 31, 2024 · c843e7c · c843e7c
2 parents dad89d3 + 0cfc51b
commit c843e7c
Show file tree

Hide file tree

Showing 7 changed files with 143 additions and 242 deletions.
diff --git a/pyrobosim/pyrobosim/core/hallway.py b/pyrobosim/pyrobosim/core/hallway.py
@@ -87,7 +87,7 @@ def __init__(
         if conn_method == "auto" or conn_method == "angle":
             theta, length = get_bearing_range(room_start.centroid, room_end.centroid)
             if conn_method == "angle":
-                length = length / np.cos(theta - conn_angle)
+                length = length * np.cos(theta - conn_angle)
                 theta = conn_angle
 
             # Calculate start and end points for the hallway

diff --git a/pyrobosim/pyrobosim/core/robot.py b/pyrobosim/pyrobosim/core/robot.py
@@ -769,12 +769,10 @@ def open_location(self):
                 )
 
         if isinstance(self.location, ObjectSpawn):
-            return self.world.open_location(self.location.parent)
-        elif isinstance(self.location, Hallway):
-            return self.world.open_hallway(self.location)
-
-        # This should not happen
-        return ExecutionResult(status=ExecutionResult.UNKNOWN)
+            loc_to_open = self.location.parent
+        else:
+            loc_to_open = self.location
+        return self.world.open_location(loc_to_open)
 
     def close_location(self):
         """
@@ -825,12 +823,10 @@ def close_location(self):
                 )
 
         if isinstance(self.location, ObjectSpawn):
-            return self.world.close_location(self.location.parent)
-        elif isinstance(self.location, Hallway):
-            return self.world.close_hallway(self.location, ignore_robots=[self])
-
-        # This should not happen
-        return ExecutionResult(status=ExecutionResult.UNKNOWN)
+            loc_to_close = self.location.parent
+        else:
+            loc_to_close = self.location
+        return self.world.close_location(loc_to_close, ignore_robots=[self])
 
     def execute_action(self, action):
         """
@@ -911,7 +907,7 @@ def execute_action(self, action):
             )
 
         if self.world.has_gui:
-            self.world.gui.set_buttons_during_action(True)
+            self.world.gui.update_button_state()
         print(f"[{self.name}] Action completed with result: {result.status.name}")
         self.current_action = None
         self.executing_action = False
@@ -979,7 +975,7 @@ def execute_plan(self, plan, delay=0.5):
             time.sleep(delay)  # Artificial delay between actions
 
         if self.world.has_gui:
-            self.world.gui.set_buttons_during_action(True)
+            self.world.gui.update_button_state()
 
         print(f"[{self.name}] Task plan completed with status: {result.status.name}")
         self.executing_plan = False

diff --git a/pyrobosim/pyrobosim/core/world.py b/pyrobosim/pyrobosim/core/world.py
@@ -284,144 +284,6 @@ def remove_hallway(self, hallway):
             self.update_bounds(entity=hallway, remove=True)
         return True
 
-    def open_hallway(self, hallway):
-        """
-        Opens a hallway between two rooms.
-
-        :param hallway: Hallway object to open.
-        :type hallway: :class:`pyrobosim.core.hallway.Hallway`
-        :return: An object describing the execution result.
-        :rtype: :class:`pyrobosim.planning.actions.ExecutionResult`
-        """
-        # Validate the input
-        if not hallway in self.hallways:
-            message = "Invalid hallway specified."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        if hallway.is_open:
-            message = f"{hallway} is already open."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        if hallway.is_locked:
-            message = f"{hallway} is locked."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        hallway.is_open = True
-        if self.has_gui:
-            self.gui.canvas.show_hallways_signal.emit()
-            self.gui.canvas.draw_signal.emit()
-        return ExecutionResult(status=ExecutionStatus.SUCCESS)
-
-    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: An object describing the execution result.
-        :rtype: :class:`pyrobosim.planning.actions.ExecutionResult`
-        """
-        # Validate the input
-        if not hallway in self.hallways:
-            message = "Invalid hallway specified."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        if not hallway.is_open:
-            message = f"{hallway} is already closed."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        if hallway.is_locked:
-            message = f"{hallway} is locked."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        for robot in [r for r in self.robots if r not in ignore_robots]:
-            if hallway.is_collision_free(robot.get_pose()):
-                message = f"Robot {robot.name} is in {hallway}. Cannot close."
-                warnings.warn(message)
-                return ExecutionResult(
-                    status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-                )
-
-        hallway.is_open = False
-        if self.has_gui:
-            self.gui.canvas.show_hallways_signal.emit()
-            self.gui.canvas.draw_signal.emit()
-        return ExecutionResult(status=ExecutionStatus.SUCCESS)
-
-    def lock_hallway(self, hallway):
-        """
-        Locks a hallway between two rooms.
-
-        :param hallway: Hallway object to lock.
-        :type hallway: :class:`pyrobosim.core.hallway.Hallway`
-        :return: An object describing the execution result.
-        :rtype: :class:`pyrobosim.planning.actions.ExecutionResult`
-        """
-        # Validate the input
-        if not hallway in self.hallways:
-            message = "Invalid hallway specified."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        if hallway.is_locked:
-            message = f"{hallway} is already locked."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        hallway.is_locked = True
-        return ExecutionResult(status=ExecutionStatus.SUCCESS)
-
-    def unlock_hallway(self, hallway):
-        """
-        Unlocks a hallway between two rooms.
-
-        :param hallway: Hallway object to unlock.
-        :type hallway: :class:`pyrobosim.core.hallway.Hallway`
-        :return: An object describing the execution result.
-        :rtype: :class:`pyrobosim.planning.actions.ExecutionResult`
-        """
-        # Validate the input
-        if not hallway in self.hallways:
-            message = "Invalid hallway specified."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        if not hallway.is_locked:
-            message = f"{hallway} is already unlocked."
-            warnings.warn(message)
-            return ExecutionResult(
-                status=ExecutionStatus.PRECONDITION_FAILURE, message=message
-            )
-
-        hallway.is_locked = False
-        return ExecutionResult(status=ExecutionStatus.SUCCESS)
-
     def add_location(self, **location_config):
         r"""
         Adds a location at the specified parent entity, usually a room.
@@ -604,15 +466,26 @@ def remove_location(self, loc):
 
     def open_location(self, location):
         """
-        Opens a storage location.
+        Opens a storage location or hallway between two rooms..
 
-        :param location: Location object to open.
-        :type location: :class:`pyrobosim.core.locations.Location`
+        :param location: Location or Hallway object to open, or its name.
+        :type location: :class:`pyrobosim.core.locations.Location`, :class:`pyrobosim.core.hallway.Hallway`, or str
         :return: An object describing the execution result.
         :rtype: :class:`pyrobosim.planning.actions.ExecutionResult`
         """
         # Validate the input
-        if not location in self.locations:
+        if isinstance(location, str):
+            location = self.get_entity_by_name(location)
+        if not (isinstance(location, Location) or isinstance(location, Hallway)):
+            message = message = (
+                f"Cannot open {location} since it is of type {type(location).__name__}."
+            )
+            warnings.warn(message)
+            return ExecutionResult(
+                status=ExecutionStatus.INVALID_ACTION, message=message
+            )
+
+        if not (location in self.locations or location in self.hallways):
             message = "Invalid location specified."
             warnings.warn(message)
             return ExecutionResult(
@@ -636,21 +509,37 @@ def open_location(self, location):
         location.is_open = True
         location.update_visualization_polygon()
         if self.has_gui:
-            self.gui.canvas.show_locations_signal.emit()
+            if isinstance(location, Hallway):
+                self.gui.canvas.show_hallways_signal.emit()
+            else:
+                self.gui.canvas.show_locations_signal.emit()
             self.gui.canvas.draw_signal.emit()
         return ExecutionResult(status=ExecutionStatus.SUCCESS)
 
-    def close_location(self, location):
+    def close_location(self, location, ignore_robots=[]):
         """
-        Close a storage location.
+        Close a storage location or hallway.
 
-        :param location: Location object to close.
-        :type location: :class:`pyrobosim.core.locations.Location`
+        :param location: Location or Hallway object to close, or its name.
+        :type location: :class:`pyrobosim.core.locations.Location`, :class:`pyrobosim.core.hallway.Hallway`, or str
+        :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: An object describing the execution result.
         :rtype: :class:`pyrobosim.planning.actions.ExecutionResult`
         """
         # Validate the input
-        if not location in self.locations:
+        if isinstance(location, str):
+            location = self.get_entity_by_name(location)
+        if not (isinstance(location, Location) or isinstance(location, Hallway)):
+            message = message = (
+                f"Cannot close {location} since it is of type {type(location).__name__}."
+            )
+            warnings.warn(message)
+            return ExecutionResult(
+                status=ExecutionStatus.INVALID_ACTION, message=message
+            )
+
+        if not (location in self.locations or location in self.hallways):
             message = "Invalid location specified."
             warnings.warn(message)
             return ExecutionResult(
@@ -671,24 +560,48 @@ def close_location(self, location):
                 status=ExecutionStatus.PRECONDITION_FAILURE, message=message
             )
 
+        is_hallway = isinstance(location, Hallway)
+        if is_hallway:
+            for robot in [r for r in self.robots if r not in ignore_robots]:
+                if location.is_collision_free(robot.get_pose()):
+                    message = f"Robot {robot.name} is in {location}. Cannot close."
+                    warnings.warn(message)
+                    return ExecutionResult(
+                        status=ExecutionStatus.PRECONDITION_FAILURE, message=message
+                    )
+
         location.is_open = False
         location.update_visualization_polygon()
         if self.has_gui:
-            self.gui.canvas.show_locations_signal.emit()
+            if is_hallway:
+                self.gui.canvas.show_hallways_signal.emit()
+            else:
+                self.gui.canvas.show_locations_signal.emit()
             self.gui.canvas.draw_signal.emit()
         return ExecutionResult(status=ExecutionStatus.SUCCESS)
 
     def lock_location(self, location):
         """
-        Locks a storage location.
+        Locks a storage location or hallway.
 
-        :param location: Location object to lock.
-        :type location: :class:`pyrobosim.core.locations.Location`
+        :param location: Location or Hallway object to lock, or its name.
+        :type location: :class:`pyrobosim.core.locations.Location`, :class:`pyrobosim.core.hallway.Hallway`, or str
         :return: An object describing the execution result.
         :rtype: :class:`pyrobosim.planning.actions.ExecutionResult`
         """
         # Validate the input
-        if not location in self.locations:
+        if isinstance(location, str):
+            location = self.get_entity_by_name(location)
+        if not (isinstance(location, Location) or isinstance(location, Hallway)):
+            message = message = (
+                f"Cannot lock {location} since it is of type {type(location).__name__}."
+            )
+            warnings.warn(message)
+            return ExecutionResult(
+                status=ExecutionStatus.INVALID_ACTION, message=message
+            )
+
+        if not (location in self.locations or location in self.hallways):
             message = "Invalid location specified."
             warnings.warn(message)
             return ExecutionResult(
@@ -707,15 +620,26 @@ def lock_location(self, location):
 
     def unlock_location(self, location):
         """
-        Unlocks a storage location.
+        Unlocks a storage location or hallway.
 
-        :param location: Location object to unlock.
-        :type location: :class:`pyrobosim.core.locations.Location`
+        :param location: Location or Hallway object to unlock, or its name.
+        :type location: :class:`pyrobosim.core.locations.Location`, :class:`pyrobosim.core.hallway.Hallway`, or str
         :return: An object describing the execution result.
         :rtype: :class:`pyrobosim.planning.actions.ExecutionResult`
         """
         # Validate the input
-        if not location in self.locations:
+        if isinstance(location, str):
+            location = self.get_entity_by_name(location)
+        if not (isinstance(location, Location) or isinstance(location, Hallway)):
+            message = message = (
+                f"Cannot unlock {location} since it is of type {type(location).__name__}."
+            )
+            warnings.warn(message)
+            return ExecutionResult(
+                status=ExecutionStatus.INVALID_ACTION, message=message
+            )
+
+        if not (location in self.locations or location in self.hallways):
             message = "Invalid location specified."
             warnings.warn(message)
             return ExecutionResult(

diff --git a/pyrobosim/pyrobosim/gui/main.py b/pyrobosim/pyrobosim/gui/main.py
@@ -217,8 +217,14 @@ def update_button_state(self):
             self.canvas.show_world_state(robot, navigating=is_moving)
         else:
             self.nav_button.setEnabled(False)
+            self.pick_button.setEnabled(False)
+            self.place_button.setEnabled(False)
+            self.detect_button.setEnabled(False)
             self.cancel_action_button.setEnabled(False)
+            self.open_button.setEnabled(True)
+            self.close_button.setEnabled(True)
             self.reset_path_planner_button.setEnabled(False)
+            self.rand_pose_button.setEnabled(False)
 
         self.canvas.draw_signal.emit()
 
@@ -325,6 +331,8 @@ def on_open_click(self):
             print(f"[{robot.name}] Opening {robot.location}")
             self.canvas.open_location(robot)
             self.update_button_state()
+        elif not robot and self.goal_textbox.text():
+            self.world.open_location(self.goal_textbox.text())
 
     def on_close_click(self):
         """Callback to close a location."""
@@ -333,6 +341,8 @@ def on_close_click(self):
             print(f"[{robot.name}] Closing {robot.location}")
             self.canvas.close_location(robot)
             self.update_button_state()
+        elif not robot and self.goal_textbox.text():
+            self.world.close_location(self.goal_textbox.text())
 
     def on_collision_polygon_toggle_click(self):
         """Callback to toggle collision polygons."""