From 8128ef6dd7b8c98242f1cf77f9cf243445778ee3 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sun, 14 Jul 2024 14:48:13 -0400 Subject: [PATCH] Add ROS service to set location state --- pyrobosim_msgs/CMakeLists.txt | 1 + pyrobosim_msgs/srv/RequestWorldState.srv | 2 +- pyrobosim_msgs/srv/SetLocationState.srv | 10 +++ pyrobosim_ros/pyrobosim_ros/ros_interface.py | 75 ++++++++++++++++++-- 4 files changed, 81 insertions(+), 7 deletions(-) create mode 100644 pyrobosim_msgs/srv/SetLocationState.srv diff --git a/pyrobosim_msgs/CMakeLists.txt b/pyrobosim_msgs/CMakeLists.txt index 27f93db2..3c23b021 100644 --- a/pyrobosim_msgs/CMakeLists.txt +++ b/pyrobosim_msgs/CMakeLists.txt @@ -29,6 +29,7 @@ set(msg_files set(srv_files "srv/RequestWorldState.srv" + "srv/SetLocationState.srv" ) set(action_files diff --git a/pyrobosim_msgs/srv/RequestWorldState.srv b/pyrobosim_msgs/srv/RequestWorldState.srv index 4226c1b0..3807f7f2 100644 --- a/pyrobosim_msgs/srv/RequestWorldState.srv +++ b/pyrobosim_msgs/srv/RequestWorldState.srv @@ -2,5 +2,5 @@ # No request --- -# Response contains a world sttae +# Response contains a world state WorldState state diff --git a/pyrobosim_msgs/srv/SetLocationState.srv b/pyrobosim_msgs/srv/SetLocationState.srv new file mode 100644 index 00000000..513e6cfe --- /dev/null +++ b/pyrobosim_msgs/srv/SetLocationState.srv @@ -0,0 +1,10 @@ +# ROS service to set the state of a location + +# Request +string location_name +bool open +bool lock + +--- +# Response +ExecutionResult result diff --git a/pyrobosim_ros/pyrobosim_ros/ros_interface.py b/pyrobosim_ros/pyrobosim_ros/ros_interface.py index 47816a8f..cdc8d1e1 100644 --- a/pyrobosim_ros/pyrobosim_ros/ros_interface.py +++ b/pyrobosim_ros/pyrobosim_ros/ros_interface.py @@ -10,9 +10,10 @@ import time from geometry_msgs.msg import Twist +from pyrobosim.core.hallway import Hallway from pyrobosim_msgs.action import ExecuteTaskAction, ExecuteTaskPlan -from pyrobosim_msgs.msg import RobotState, LocationState, ObjectState -from pyrobosim_msgs.srv import RequestWorldState +from pyrobosim_msgs.msg import ExecutionResult, RobotState, LocationState, ObjectState +from pyrobosim_msgs.srv import RequestWorldState, SetLocationState from .ros_conversions import ( execution_result_to_ros, pose_from_ros, @@ -97,7 +98,7 @@ def __init__( callback_group=ReentrantCallbackGroup(), ) - # World state service server + # World state service servers self.world_state_srv = self.create_service( RequestWorldState, "request_world_state", @@ -105,6 +106,13 @@ def __init__( callback_group=ReentrantCallbackGroup(), ) + self.set_location_state_srv = self.create_service( + SetLocationState, + "set_location_state", + self.set_location_state_callback, + callback_group=ReentrantCallbackGroup(), + ) + # Initialize robot specific interface lists self.robot_command_subs = [] self.robot_state_pubs = [] @@ -401,11 +409,11 @@ def world_state_callback(self, request, response): Returns the world state as a response to a service request. :param request: The service request. - :type request: :class:`pyrobosim_msgs.srv._request_world_state.RequestWorldState_Request` + :type request: :class:`pyrobosim_msgs.srv.RequestWorldState.Request` :param response: The unmodified service response. - :type response: :class:`pyrobosim_msgs.srv._request_world_state.RequestWorldState_Response` + :type response: :class:`pyrobosim_msgs.srv.RequestWorldState.Response` :return: The modified service response containing the world state. - :rtype: :class:`pyrobosim_msgs.srv._request_world_state.RequestWorldState_Response` + :rtype: :class:`pyrobosim_msgs.srv.RequestWorldState.Response` """ self.get_logger().info("Received world state request.") @@ -433,6 +441,61 @@ def world_state_callback(self, request, response): return response + def set_location_state_callback(self, request, response): + """ + Sets the state of a location in the world as a response to a service request. + + :param request: The service request. + :type request: :class:`pyrobosim_msgs.srv.SetLocationState.Request` + :param response: The unmodified service response. + :type response: :class:`pyrobosim_msgs.srv.SetLocationState.Response` + :return: The modified service response containing result of setting the location state. + :rtype: :class:`pyrobosim_msgs.srv.RequestWorldState.Response` + """ + self.get_logger().info("Received location state setting request.") + + # Check if the entity exists. + entity = self.world.get_entity_by_name(request.location_name) + if not entity: + message = f"No location matching query: {request.location_name}" + self.get_logger().warn(message) + response.result.status = ExecutionResult.INVALID_ACTION + response.result.message = message + return response + + if 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 + + # 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 + return response + def update_world_from_state_msg(world, msg): """