diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index d8709ee75..ccdca186a 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -33,11 +33,14 @@ from action_msgs.msg._goal_status_array import GoalStatusArray from action_msgs.srv import CancelGoal from builtin_interfaces.msg import Time + +from rclpy.clock import Clock from rclpy.executors import await_or_execute from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import qos_profile_action_status_default from rclpy.qos import qos_profile_services_default from rclpy.qos import QoSProfile +from rclpy.service_introspection import ServiceIntrospectionState from rclpy.task import Future from rclpy.type_support import Action from rclpy.type_support import check_for_type_support @@ -671,6 +674,24 @@ def wait_for_server(self, timeout_sec: Optional[float] = None) -> bool: return self.server_is_ready() + def configure_introspection( + self, clock: Clock, + service_event_qos_profile: QoSProfile, + introspection_state: ServiceIntrospectionState + ) -> None: + """ + Configure action client introspection. + + :param clock: Clock to use for generating timestamps. + :param service_event_qos_profile: QoSProfile to use when creating service event publisher. + :param introspection_state: ServiceIntrospectionState to set introspection. + """ + with self._client_handle: + self._client_handle.configure_introspection(clock.handle, + service_event_qos_profile + .get_c_qos_profile(), + introspection_state) + def destroy(self) -> None: """Destroy the underlying action client handle.""" self._client_handle.destroy_when_not_in_use() diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index f2a11b851..c32ae417c 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -24,11 +24,14 @@ from action_msgs.msg import GoalInfo, GoalStatus from action_msgs.srv._cancel_goal import CancelGoal + +from rclpy.clock import Clock from rclpy.executors import await_or_execute from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import qos_profile_action_status_default from rclpy.qos import qos_profile_services_default from rclpy.qos import QoSProfile +from rclpy.service_introspection import ServiceIntrospectionState from rclpy.task import Future from rclpy.task import Task from rclpy.type_support import (Action, check_for_type_support, FeedbackMessage, FeedbackT, @@ -706,6 +709,23 @@ def register_execute_callback( raise TypeError('Failed to register goal execution callback: not callable') self._execute_callback = execute_callback + def configure_introspection( + self, clock: Clock, + service_event_qos_profile: QoSProfile, + introspection_state: ServiceIntrospectionState + ) -> None: + """ + Configure action server introspection. + + :param clock: Clock to use for generating timestamps. + :param service_event_qos_profile: QoSProfile to use when creating service event publisher. + :param introspection_state: ServiceIntrospectionState to set introspection. + """ + with self._handle: + self._handle.configure_introspection(clock.handle, + service_event_qos_profile.get_c_qos_profile(), + introspection_state) + def destroy(self) -> None: """Destroy the underlying action server handle.""" for goal_handle in self._goal_handles.values(): diff --git a/rclpy/src/rclpy/action_client.cpp b/rclpy/src/rclpy/action_client.cpp index a52864ab5..dfc4af8df 100644 --- a/rclpy/src/rclpy/action_client.cpp +++ b/rclpy/src/rclpy/action_client.cpp @@ -50,9 +50,9 @@ ActionClient::ActionClient( const rmw_qos_profile_t & status_topic_qos) : node_(node) { - rosidl_action_type_support_t * ts = + action_type_support_ = static_cast(common_get_type_support(pyaction_type)); - if (!ts) { + if (!action_type_support_) { throw py::error_already_set(); } @@ -86,7 +86,7 @@ ActionClient::ActionClient( rcl_ret_t ret = rcl_action_client_init( rcl_action_client_.get(), node_.rcl_ptr(), - ts, + action_type_support_, action_name, &action_client_ops); if (RCL_RET_ACTION_NAME_INVALID == ret) { @@ -269,6 +269,25 @@ ActionClient::is_ready(WaitSet & wait_set) return result_tuple; } +void +ActionClient::configure_introspection( + Clock & clock, py::object pyqos_service_event_pub, + rcl_service_introspection_state_t introspection_state) +{ + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = + pyqos_service_event_pub.is_none() ? rcl_publisher_get_default_options().qos : + pyqos_service_event_pub.cast(); + + rcl_ret_t ret = rcl_action_client_configure_action_introspection( + rcl_action_client_.get(), node_.rcl_ptr(), clock.rcl_ptr(), + action_type_support_, pub_opts, introspection_state); + + if (RCL_RET_OK != ret) { + throw RCLError("failed to configure action client introspection"); + } +} + void define_action_client(py::object module) { @@ -317,6 +336,9 @@ define_action_client(py::object module) "Check if an action entity has any ready wait set entities.") .def( "take_status", &ActionClient::take_status, - "Take an action status response."); + "Take an action status response.") + .def( + "configure_introspection", &ActionClient::configure_introspection, + "Configure whether internal client introspection is enabled"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/action_client.hpp b/rclpy/src/rclpy/action_client.hpp index 49d616e4c..7448e9b3b 100644 --- a/rclpy/src/rclpy/action_client.hpp +++ b/rclpy/src/rclpy/action_client.hpp @@ -170,6 +170,17 @@ class ActionClient : public Destroyable, public std::enable_shared_from_this rcl_action_client_; + const rosidl_action_type_support_t * action_type_support_; }; /// Define a pybind11 wrapper for an rclpy::ActionClient /** diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp index 0241bd0f2..891bf6250 100644 --- a/rclpy/src/rclpy/action_server.cpp +++ b/rclpy/src/rclpy/action_server.cpp @@ -58,9 +58,9 @@ ActionServer::ActionServer( { rcl_clock_t * clock = rclpy_clock.rcl_ptr(); - rosidl_action_type_support_t * ts = static_cast( + action_type_support_ = static_cast( common_get_type_support(pyaction_type)); - if (!ts) { + if (!action_type_support_) { throw py::error_already_set(); } @@ -96,7 +96,7 @@ ActionServer::ActionServer( rcl_action_server_.get(), node_.rcl_ptr(), clock, - ts, + action_type_support_, action_name, &action_server_ops); if (RCL_RET_ACTION_NAME_INVALID == ret) { @@ -367,6 +367,25 @@ ActionServer::expire_goals(int64_t max_num_goals) return result_tuple; } +void +ActionServer::configure_introspection( + Clock & clock, py::object pyqos_service_event_pub, + rcl_service_introspection_state_t introspection_state) +{ + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = + pyqos_service_event_pub.is_none() ? rcl_publisher_get_default_options().qos : + pyqos_service_event_pub.cast(); + + rcl_ret_t ret = rcl_action_server_configure_action_introspection( + rcl_action_server_.get(), node_.rcl_ptr(), clock.rcl_ptr(), + action_type_support_, pub_opts, introspection_state); + + if (RCL_RET_OK != ret) { + throw RCLError("failed to configure action server introspection"); + } +} + void define_action_server(py::object module) { @@ -424,7 +443,10 @@ define_action_server(py::object module) "Check if an action entity has any ready wait set entities.") .def( "add_to_waitset", &ActionServer::add_to_waitset, - "Add an action entity to a wait set."); + "Add an action entity to a wait set.") + .def( + "configure_introspection", &ActionServer::configure_introspection, + "Configure whether internal service introspection is enabled"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp index edc722e70..c855f0f0e 100644 --- a/rclpy/src/rclpy/action_server.hpp +++ b/rclpy/src/rclpy/action_server.hpp @@ -248,6 +248,17 @@ class ActionServer : public Destroyable, public std::enable_shared_from_this rcl_action_server_; + const rosidl_action_type_support_t * action_type_support_; }; /// Define a pybind11 wrapper for an rclpy::ActionServer /** diff --git a/rclpy/test/test_action_client.py b/rclpy/test/test_action_client.py index 8e98685c8..e3e6aae7f 100644 --- a/rclpy/test/test_action_client.py +++ b/rclpy/test/test_action_client.py @@ -20,7 +20,10 @@ from rclpy.action import ActionClient from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor -from rclpy.qos import qos_profile_action_status_default +from rclpy.qos import qos_profile_action_status_default, qos_profile_system_default +from rclpy.service_introspection import ServiceIntrospectionState + +from service_msgs.msg import ServiceEventInfo from test_msgs.action import Fibonacci @@ -368,6 +371,92 @@ def test_different_type_raises(self) -> None: finally: ac.destroy() + def test_action_introspection_default_status(self) -> None: + ac: ActionClient = ActionClient(self.node, Fibonacci, 'fibonacci') + + self.event_messages = [] + + def sub_callback(msg): + self.event_messages.append(msg) + + # There is no need to check if introspection is enabled for all internal services, + # as the implementation in the RCL interface operates on the three internal services + # simultaneously. So only check send_goal service event. + send_goal_service_event_sub = self.node.create_subscription( + Fibonacci.Impl.SendGoalService.Event, + '/fibonacci/_action/send_goal/_service_event', + sub_callback, 3) + + try: + self.assertTrue(ac.wait_for_server(timeout_sec=2.0)) + + # Send a goal + goal_future = ac.send_goal_async(Fibonacci.Goal()) + rclpy.spin_until_future_complete(self.node, goal_future, self.executor) + self.assertTrue(goal_future.done()) + + # By default, action client introspection is disabled. + # So no service event message can be received. + start = time.monotonic() + end = start + 1.0 + while len(self.event_messages) < 1: + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + now = time.monotonic() + if now >= end: + break + + self.assertEqual(len(self.event_messages), 0) + finally: + self.node.destroy_subscription(send_goal_service_event_sub) + ac.destroy() + + def test_configure_introspection_content(self) -> None: + ac: ActionClient = ActionClient(self.node, Fibonacci, 'fibonacci') + + self.event_messages = [] + + def sub_callback(msg): + self.event_messages.append(msg) + + # There is no need to check if introspection is enabled for all internal services, + # as the implementation in the RCL interface operates on the three internal services + # simultaneously. So only check send_goal service event. + send_goal_service_event_sub = self.node.create_subscription( + Fibonacci.Impl.SendGoalService.Event, + '/fibonacci/_action/send_goal/_service_event', + sub_callback, 3) + + try: + ac.configure_introspection(self.node.get_clock(), + qos_profile_system_default, + ServiceIntrospectionState.CONTENTS) + + self.assertTrue(ac.wait_for_server(timeout_sec=2.0)) + + # Send a goal + goal_future = ac.send_goal_async(Fibonacci.Goal()) + rclpy.spin_until_future_complete(self.node, goal_future, self.executor) + self.assertTrue(goal_future.done()) + + start = time.monotonic() + end = start + 5.0 + while len(self.event_messages) < 1: + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + now = time.monotonic() + self.assertTrue(now < end) + + self.assertEqual(len(self.event_messages), 1) + + self.assertEqual(self.event_messages[0].info.event_type, ServiceEventInfo.REQUEST_SENT) + + # For ServiceIntrospectionState.CONTENTS mode, the request or response section must + # contain data. In this case, the request section must contain data. + self.assertEqual(len(self.event_messages[0].request), 1) + self.assertEqual(len(self.event_messages[0].response), 0) + finally: + self.node.destroy_subscription(send_goal_service_event_sub) + ac.destroy() + if __name__ == '__main__': unittest.main() diff --git a/rclpy/test/test_action_server.py b/rclpy/test/test_action_server.py index 648dd30f6..864e39baa 100644 --- a/rclpy/test/test_action_server.py +++ b/rclpy/test/test_action_server.py @@ -23,6 +23,10 @@ from rclpy.action import ActionServer, CancelResponse, GoalResponse from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from rclpy.qos import qos_profile_system_default +from rclpy.service_introspection import ServiceIntrospectionState + +from service_msgs.msg import ServiceEventInfo from test_msgs.action import Fibonacci @@ -684,6 +688,126 @@ def execute_with_feedback(goal_handle): finally: action_server.destroy() + def test_action_introspection_default_status(self) -> None: + goal_order = 10 + + def goal_callback(goal): + nonlocal goal_order + self.assertEqual(goal.order, goal_order) + return GoalResponse.REJECT + + def handle_accepted_callback(goal_handle): + # Since the goal is rejected, we don't expect this function to be called + self.assertFalse(True) + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + goal_callback=goal_callback, + handle_accepted_callback=handle_accepted_callback, + ) + + # There is no need to check if introspection is enabled for all internal services, + # as the implementation in the RCL interface operates on the three internal services + # simultaneously. + + self.event_messages = [] + + def sub_callback(msg): + self.event_messages.append(msg) + + send_goal_service_event_sub = self.node.create_subscription( + Fibonacci.Impl.SendGoalService.Event, + '/fibonacci/_action/send_goal/_service_event', + sub_callback, 3) + + goal_msg = Fibonacci.Impl.SendGoalService.Request() + goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + goal_msg.goal.order = goal_order + future = self.mock_action_client.send_goal(goal_msg) + rclpy.spin_until_future_complete(self.node, future, self.executor) + self.assertFalse(future.result().accepted) + + # By default, action client introspection is disabled. So no service event message can + # be received. + start = time.monotonic() + end = start + 1.0 + while len(self.event_messages) < 1: + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + now = time.monotonic() + if now >= end: + break + + self.assertEqual(len(self.event_messages), 0) + + send_goal_service_event_sub.destroy() + action_server.destroy() + + def test_configure_introspection_content(self) -> None: + goal_order = 10 + + def goal_callback(goal): + nonlocal goal_order + self.assertEqual(goal.order, goal_order) + return GoalResponse.REJECT + + def handle_accepted_callback(goal_handle): + # Since the goal is rejected, we don't expect this function to be called + self.assertFalse(True) + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + goal_callback=goal_callback, + handle_accepted_callback=handle_accepted_callback, + ) + + action_server.configure_introspection(self.node.get_clock(), qos_profile_system_default, + ServiceIntrospectionState.CONTENTS) + + # There is no need to check if introspection is enabled for all internal services, + # as the implementation in the RCL interface operates on the three internal services + # simultaneously. + + self.event_messages = [] + + def sub_callback(msg): + self.event_messages.append(msg) + + send_goal_service_event_sub = self.node.create_subscription( + Fibonacci.Impl.SendGoalService.Event, + '/fibonacci/_action/send_goal/_service_event', + sub_callback, 3) + + goal_msg = Fibonacci.Impl.SendGoalService.Request() + goal_msg.goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + goal_msg.goal.order = goal_order + future = self.mock_action_client.send_goal(goal_msg) + rclpy.spin_until_future_complete(self.node, future, self.executor) + self.assertFalse(future.result().accepted) + + start = time.monotonic() + end = start + 5.0 + while len(self.event_messages) < 1: + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + now = time.monotonic() + self.assertTrue(now < end) + self.assertEqual(len(self.event_messages), 1) + + self.assertEqual(self.event_messages[0].info.event_type, ServiceEventInfo.REQUEST_RECEIVED) + + # For ServiceIntrospectionState.CONTENTS mode, the request or response section must + # contain data. In this case, the request section must contain data. + self.assertEqual(len(self.event_messages[0].request), 1) + self.assertEqual(len(self.event_messages[0].response), 0) + + send_goal_service_event_sub.destroy() + action_server.destroy() + if __name__ == '__main__': unittest.main()