Add new interfaces to enable intropsection for action

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()

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():

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<rosidl_action_type_support_t *>(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<rmw_qos_profile_t>();
+
+  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

rclpy/src/rclpy/action_client.hpp

@@ -170,6 +170,17 @@ class ActionClient : public Destroyable, public std::enable_shared_from_this<Act
   py::tuple
   take_result_response(py::object pymsg_type);
 
+  /// Configure action client introspection
+  /**
+   * \param[in] clock clock to use for service event timestamps
+   * \param[in] pyqos_service_event_pub QoSProfile python object for the service event publisher
+   * \param[in] introspection_state which state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock & clock, py::object pyqos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state);
+
   /// Get the number of wait set entities that make up an action entity.
   /**
    * \return Tuple containing the number of wait set entities:
@@ -229,6 +240,7 @@ class ActionClient : public Destroyable, public std::enable_shared_from_this<Act
 private:
   Node node_;
   std::shared_ptr<rcl_action_client_t> rcl_action_client_;
+  const rosidl_action_type_support_t * action_type_support_;
 };
 /// Define a pybind11 wrapper for an rclpy::ActionClient
 /**

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<rosidl_action_type_support_t *>(
+  action_type_support_ = static_cast<rosidl_action_type_support_t *>(
     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<rmw_qos_profile_t>();
+
+  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

rclpy/src/rclpy/action_server.hpp

@@ -248,13 +248,25 @@ class ActionServer : public Destroyable, public std::enable_shared_from_this<Act
     return rcl_action_server_.get();
   }
 
+  /// Configure action server introspection
+  /**
+   * \param[in] clock clock to use for service event timestamps
+   * \param[in] pyqos_service_event_pub QoSProfile python object for the service event publisher
+   * \param[in] introspection_state which state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock & clock, py::object pyqos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state);
+
   /// Force an early destruction of this object
   void
   destroy() override;
 
 private:
   Node node_;
   std::shared_ptr<rcl_action_server_t> rcl_action_server_;
+  const rosidl_action_type_support_t * action_type_support_;
 };
 /// Define a pybind11 wrapper for an rclpy::ActionServer
 /**

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()