From 5404650a5f6010c2811aabfc947f94a536fb945f Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 30 Aug 2024 21:36:29 -0400 Subject: [PATCH 1/2] Fix hallway length calculation --- pyrobosim/pyrobosim/core/hallway.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyrobosim/pyrobosim/core/hallway.py b/pyrobosim/pyrobosim/core/hallway.py index 85adccd3..914dfd9e 100644 --- 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 From 0cfc51b33c5c25313e4dfc992e47d69a20c29e21 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sat, 31 Aug 2024 16:00:17 -0400 Subject: [PATCH 2/2] Allow opening and closing locations globally from GUI (#254) --- pyrobosim/pyrobosim/core/robot.py | 24 +- pyrobosim/pyrobosim/core/world.py | 238 +++++++------------ pyrobosim/pyrobosim/gui/main.py | 10 + pyrobosim/test/core/test_hallway.py | 18 +- pyrobosim/test/core/test_robot.py | 5 +- pyrobosim_ros/pyrobosim_ros/ros_interface.py | 88 +++---- 6 files changed, 142 insertions(+), 241 deletions(-) diff --git a/pyrobosim/pyrobosim/core/robot.py b/pyrobosim/pyrobosim/core/robot.py index 249f57a2..a29650d6 100644 --- 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 index dc47c9bb..8afe372b 100644 --- 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 index bdfe4be0..0971c401 100644 --- 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.""" diff --git a/pyrobosim/test/core/test_hallway.py b/pyrobosim/test/core/test_hallway.py index 17743d25..139a3dd8 100644 --- a/pyrobosim/test/core/test_hallway.py +++ b/pyrobosim/test/core/test_hallway.py @@ -98,27 +98,27 @@ def test_add_hallway_open_close_lock_unlock(self): # When trying to open the hallway, it should say it's already open. with pytest.warns(UserWarning): - result = self.test_world.open_hallway(hallway) + result = self.test_world.open_location(hallway) assert result.status == ExecutionStatus.PRECONDITION_FAILURE assert result.message == "Hallway: hall_room_start_room_end is already open." assert hallway.is_open assert not hallway.is_locked # Closing should work - result = self.test_world.close_hallway(hallway) + result = self.test_world.close_location(hallway) assert result.is_success() assert not hallway.is_open assert not hallway.is_locked # Locking should work - result = self.test_world.lock_hallway(hallway) + result = self.test_world.lock_location(hallway) assert result.is_success() assert not hallway.is_open assert hallway.is_locked # Opening should not work due to being locked with pytest.warns(UserWarning): - result = self.test_world.open_hallway(hallway) + result = self.test_world.open_location(hallway) assert result.status == ExecutionStatus.PRECONDITION_FAILURE assert result.message == "Hallway: hall_room_start_room_end is locked." assert not hallway.is_open @@ -126,7 +126,7 @@ def test_add_hallway_open_close_lock_unlock(self): # Closing should not work due to already being closed with pytest.warns(UserWarning): - result = self.test_world.close_hallway(hallway) + result = self.test_world.close_location(hallway) assert result.status == ExecutionStatus.PRECONDITION_FAILURE assert result.message == "Hallway: hall_room_start_room_end is already closed." assert not hallway.is_open @@ -134,21 +134,21 @@ def test_add_hallway_open_close_lock_unlock(self): # Locking should not work due to already being locked with pytest.warns(UserWarning): - result = self.test_world.lock_hallway(hallway) + result = self.test_world.lock_location(hallway) assert result.status == ExecutionStatus.PRECONDITION_FAILURE assert result.message == "Hallway: hall_room_start_room_end is already locked." assert not hallway.is_open assert hallway.is_locked # Unlocking should work - result = self.test_world.unlock_hallway(hallway) + result = self.test_world.unlock_location(hallway) assert result.is_success() assert not hallway.is_open assert not hallway.is_locked # Unlocking should not work due to already being unlocked with pytest.warns(UserWarning): - result = self.test_world.unlock_hallway(hallway) + result = self.test_world.unlock_location(hallway) assert result.status == ExecutionStatus.PRECONDITION_FAILURE assert ( result.message == "Hallway: hall_room_start_room_end is already unlocked." @@ -157,7 +157,7 @@ def test_add_hallway_open_close_lock_unlock(self): assert not hallway.is_locked # Opening should work - result = self.test_world.open_hallway(hallway) + result = self.test_world.open_location(hallway) assert result.is_success() assert hallway.is_open assert not hallway.is_locked diff --git a/pyrobosim/test/core/test_robot.py b/pyrobosim/test/core/test_robot.py index c31b32b8..56770a67 100644 --- a/pyrobosim/test/core/test_robot.py +++ b/pyrobosim/test/core/test_robot.py @@ -219,7 +219,6 @@ def test_robot_nav_validation(self): ), ) robot.world = self.test_world - robot.location = "kitchen" # Plan a path. robot.set_pose(init_pose) @@ -229,7 +228,7 @@ def test_robot_nav_validation(self): def close_all_hallways(): time.sleep(0.5) for hallway in self.test_world.hallways: - self.test_world.close_hallway(hallway, ignore_robots=[robot]) + self.test_world.close_location(hallway, ignore_robots=[robot]) threading.Thread(target=close_all_hallways).start() @@ -245,7 +244,7 @@ def close_all_hallways(): # Now open the hallways and try again, which should succeed. for hallway in self.test_world.hallways: - self.test_world.open_hallway(hallway) + self.test_world.open_location(hallway) path = robot.plan_path(goal=goal_pose) result = robot.follow_path(path) assert result.status == ExecutionStatus.SUCCESS diff --git a/pyrobosim_ros/pyrobosim_ros/ros_interface.py b/pyrobosim_ros/pyrobosim_ros/ros_interface.py index 7a53d266..5a9db690 100644 --- a/pyrobosim_ros/pyrobosim_ros/ros_interface.py +++ b/pyrobosim_ros/pyrobosim_ros/ros_interface.py @@ -14,8 +14,6 @@ from rclpy.node import Node from geometry_msgs.msg import Twist -from pyrobosim.core.hallway import Hallway -from pyrobosim.core.locations import Location from pyrobosim_msgs.action import ( DetectObjects, ExecuteTaskAction, @@ -726,63 +724,37 @@ def set_location_state_callback(self, request, response): response.result.message = message return response - if isinstance(entity, Location): - # Try open or close the location if its status needs to be toggled. - if request.open != entity.is_open: - if request.open: - result = self.world.open_location(entity) - else: - result = self.world.close_location(entity) - - if not result.is_success(): - response.result = execution_result_to_ros(result) - return response - - # Try lock or unlock the location if its status needs to be toggled. - if request.lock != entity.is_locked: - if request.lock: - result = self.world.lock_location(entity) - else: - result = self.world.unlock_location(entity) - - if not result.is_success(): - response.result = execution_result_to_ros(result) - return response - - response.result.status = ExecutionResult.SUCCESS - return response - - elif isinstance(entity, Hallway): - # Try open or close the hallway if its status needs to be toggled. - if request.open != entity.is_open: - if request.open: - result = self.world.open_hallway(entity) - else: - result = self.world.close_hallway(entity) - - if not result.is_success(): - response.result = execution_result_to_ros(result) - return response - - # Try lock or unlock the hallway if its status needs to be toggled. - if request.lock != entity.is_locked: - if request.lock: - result = self.world.lock_hallway(entity) - else: - result = self.world.unlock_hallway(entity) - - if not result.is_success(): - response.result = execution_result_to_ros(result) - return response - - response.result.status = ExecutionResult.SUCCESS - return response + # Initialize the result in case no actions need to happen. + response.result = ExecutionResult( + status=ExecutionResult.SUCCESS, message="No action taken." + ) - # If no valid entity type is reached, we should fail here. - message = f"Cannot set state for {entity.name} since it is of type {type(entity).__name__}." - self.get_logger().warn(message) - response.result.status = ExecutionResult.INVALID_ACTION - response.result.message = message + # Try open or close the location if its status needs to be toggled. + result = None + if request.open != entity.is_open: + if request.open: + result = self.world.open_location(entity) + else: + result = self.world.close_location(entity) + + if not result.is_success(): + response.result = execution_result_to_ros(result) + return response + + # Try lock or unlock the location if its status needs to be toggled. + if request.lock != entity.is_locked: + if request.lock: + result = self.world.lock_location(entity) + else: + result = self.world.unlock_location(entity) + + if not result.is_success(): + response.result = execution_result_to_ros(result) + return response + + # If we made it here, return the latest result. + if result is not None: + response.result = execution_result_to_ros(result) return response