Add new interfaces to enable intropsection for action

rcl/include/rcl/client.h

@@ -501,6 +501,7 @@ rcl_client_set_on_new_response_callback(
 
 /// Configures service introspection features for the client.
 /**
+ * \anchor rcl_client_configure_service_introspection
  * Enables or disables service introspection features for this client.
  * If the introspection state is RCL_SERVICE_INTROSPECTION_OFF, then introspection will
  * be disabled.  If the state is RCL_SERVICE_INTROSPECTION_METADATA, the client metadata

rcl/include/rcl/service.h

@@ -533,6 +533,7 @@ rcl_service_set_on_new_request_callback(
 
 /// Configure service introspection features for the service.
 /**
+ * \anchor rcl_service_configure_service_introspection
  * Enables or disables service introspection features for this service.
  * If the introspection state is RCL_SERVICE_INTROSPECTION_OFF, then introspection will
  * be disabled.  If the state is RCL_SERVICE_INTROSPECTION_METADATA, the client metadata

rcl_action/CMakeLists.txt

@@ -103,6 +103,7 @@ if(BUILD_TESTING)
   endif()
 
   ament_add_gtest_executable(test_action_communication test/rcl_action/test_action_communication.cpp)
+  target_include_directories(test_action_communication PUBLIC src)
   target_link_libraries(test_action_communication
     ${PROJECT_NAME}
     ${action_msgs_TARGETS}

rcl_action/include/rcl_action/action_client.h

@@ -25,6 +25,8 @@ extern "C"
 #include "rcl/event_callback.h"
 #include "rcl/macros.h"
 #include "rcl/node.h"
+#include "rcl/publisher.h"
+#include "rcl/service_introspection.h"
 
 
 /// Internal action client implementation struct.
@@ -742,6 +744,43 @@ bool
 rcl_action_client_is_valid(
   const rcl_action_client_t * action_client);
 
+/// Configures service introspection features for all internal service clients of the action client.
+/**
+ * For the internal goal, cancel, and result clients of the action client, call
+ * \ref rcl_client_configure_service_introspection separately for each.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | Maybe [1]
+ * Lock-Free          | Maybe [1]
+ * <i>[1] rmw implementation defined</i>
+ *
+ * \param[in] action_client action client on which to configure internal service introspection
+ * \param[in] node valid rcl_node_t to use to create the introspection publisher
+ * \param[in] clock valid rcl_clock_t to use to generate the introspection timestamps
+ * \param[in] type_support type support library associated with this action client
+ * \param[in] publisher_options options to use when creating the introspection publisher
+ * \param[in] introspection_state rcl_service_introspection_state_t describing whether
+ *            introspection should be OFF, METADATA, or CONTENTS
+ * \return #RCL_RET_OK if the call was successful, or
+ * \return #RCL_RET_ERROR if calling rcl_client_configure_service_introspection for each internal
+ *         client doesn't return RCL_RET_OK, or
+ * \return #RCL_RET_INVALID_ARGUMENT if the given node or clock or type_support is invalid
+ */
+RCL_ACTION_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_action_client_configure_internal_service_introspection(
+  rcl_action_client_t * action_client,
+  rcl_node_t * node,
+  rcl_clock_t * clock,
+  const rosidl_action_type_support_t * type_support,
+  const rcl_publisher_options_t publisher_options,
+  rcl_service_introspection_state_t introspection_state);
+
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t

rcl_action/include/rcl_action/action_server.h

@@ -26,6 +26,8 @@ extern "C"
 #include "rcl/event_callback.h"
 #include "rcl/macros.h"
 #include "rcl/node.h"
+#include "rcl/publisher.h"
+#include "rcl/service_introspection.h"
 #include "rcl/time.h"
 #include "rcl/timer.h"
 
@@ -947,6 +949,43 @@ RCL_WARN_UNUSED
 bool
 rcl_action_server_is_valid_except_context(const rcl_action_server_t * action_server);
 
+/// Configure service introspection features for all internal service servers of the action server
+/**
+ * For the internal goal, cancel, and result services of the action server, call
+ * \ref rcl_service_configure_service_introspection separately for each.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | Maybe [1]
+ * Lock-Free          | Maybe [1]
+ * <i>[1] rmw implementation defined</i>
+ *
+ * \param[in] action_server The action server on which to configure internal service introspection
+ * \param[in] node valid rcl_node_t to use to create the introspection publisher
+ * \param[in] clock valid rcl_clock_t to use to generate the introspection timestamps
+ * \param[in] type_support type support library associated with this action server
+ * \param[in] publisher_options options to use when creating the introspection publisher
+ * \param[in] introspection_state rcl_service_introspection_state_t describing whether
+ *            introspection should be OFF, METADATA, or CONTENTS
+ * \return #RCL_RET_OK if the call was successful, or
+ * \return #RCL_RET_ERROR if calling rcl_service_configure_service_introspection for each internal
+ *         services doesn't return RCL_RET_OK, or
+ * \return #RCL_RET_INVALID_ARGUMENT if the given node or clock or type_support is invalid
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_action_server_configure_internal_service_introspection(
+  rcl_action_server_t * action_server,
+  rcl_node_t * node,
+  rcl_clock_t * clock,
+  const rosidl_action_type_support_t * type_support,
+  const rcl_publisher_options_t publisher_options,
+  rcl_service_introspection_state_t introspection_state);
+
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t

rcl_action/src/rcl_action/action_client.c

@@ -752,6 +752,41 @@ rcl_action_client_set_status_subscription_callback(
     user_data);
 }
 
+#define CLIENT_CONFIGURE_SERVICE_INTROSPECTION(TYPE, STATE) \
+  if (rcl_client_configure_service_introspection( \
+      &action_client->impl->TYPE ## _client, \
+      node, \
+      clock, \
+      type_support->TYPE ## _service_type_support, \
+      publisher_options, \
+      STATE) != RCL_RET_OK) \
+  { \
+    ret = RCL_RET_ERROR; \
+  }
+
+rcl_ret_t
+rcl_action_client_configure_internal_service_introspection(
+  rcl_action_client_t * action_client,
+  rcl_node_t * node,
+  rcl_clock_t * clock,
+  const rosidl_action_type_support_t * type_support,
+  const rcl_publisher_options_t publisher_options,
+  rcl_service_introspection_state_t introspection_state)
+{
+  if (!rcl_action_client_is_valid(action_client)) {
+    return RCL_RET_ACTION_CLIENT_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
+
+  rcl_ret_t ret = RCL_RET_OK;
+  CLIENT_CONFIGURE_SERVICE_INTROSPECTION(goal, introspection_state);
+  CLIENT_CONFIGURE_SERVICE_INTROSPECTION(cancel, introspection_state);
+  CLIENT_CONFIGURE_SERVICE_INTROSPECTION(result, introspection_state);
+  return ret;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl_action/src/rcl_action/action_server.c

@@ -1240,6 +1240,41 @@ rcl_action_server_set_cancel_service_callback(
     user_data);
 }
 
+#define SERVER_CONFIGURE_SERVICE_INTROSPECTION(TYPE, STATE) \
+  if (rcl_service_configure_service_introspection( \
+      &action_server->impl->TYPE ## _service, \
+      node, \
+      clock, \
+      type_support->TYPE ## _service_type_support, \
+      publisher_options, \
+      STATE) != RCL_RET_OK) \
+  { \
+    ret = RCL_RET_ERROR; \
+  }
+
+rcl_ret_t
+rcl_action_server_configure_internal_service_introspection(
+  rcl_action_server_t * action_server,
+  rcl_node_t * node,
+  rcl_clock_t * clock,
+  const rosidl_action_type_support_t * type_support,
+  const rcl_publisher_options_t publisher_options,
+  rcl_service_introspection_state_t introspection_state)
+{
+  if (!rcl_action_server_is_valid_except_context(action_server)) {
+    return RCL_RET_ACTION_SERVER_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
+
+  rcl_ret_t ret = RCL_RET_OK;
+  SERVER_CONFIGURE_SERVICE_INTROSPECTION(goal, introspection_state);
+  SERVER_CONFIGURE_SERVICE_INTROSPECTION(cancel, introspection_state);
+  SERVER_CONFIGURE_SERVICE_INTROSPECTION(result, introspection_state);
+  return ret;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl_action/test/rcl_action/test_action_client.cpp

@@ -20,6 +20,7 @@
 #include "rcl_action/action_client_impl.h"
 
 #include "rcl/error_handling.h"
+#include "rcl/graph.h"
 #include "rcl/rcl.h"
 #include "rcutils/testing/fault_injection.h"
 
@@ -223,14 +224,23 @@ class TestActionClientFixture : public TestActionClientBaseFixture
   {
     TestActionClientBaseFixture::SetUp();
     this->action_client = rcl_action_get_zero_initialized_client();
-    const rosidl_action_type_support_t * action_typesupport =
-      ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
+    action_typesupport = ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
     this->action_client_options = rcl_action_client_get_default_options();
     rcl_ret_t ret = rcl_action_client_init(
       &this->action_client, &this->node, action_typesupport,
       this->action_name, &this->action_client_options);
     ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
     this->invalid_action_client = rcl_action_get_zero_initialized_client();
+
+    rcl_allocator_t allocator = rcl_get_default_allocator();
+    ret = rcl_clock_init(RCL_ROS_TIME, &this->clock, &allocator);
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+    send_goal_service_event_topic_name = std::string(action_client.impl->remapped_action_name) +
+      "/_action/send_goal" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
+    cancel_goal_service_event_topic_name = std::string(action_client.impl->remapped_action_name) +
+      "/_action/cancel_goal" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
+    get_result_service_event_topic_name = std::string(action_client.impl->remapped_action_name) +
+      "/_action/get_result" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
   }
 
   void TearDown() override
@@ -240,10 +250,51 @@ class TestActionClientFixture : public TestActionClientBaseFixture
     TestActionClientBaseFixture::TearDown();
   }
 
+  void check_set_services_introspection(
+    rcl_service_introspection_state_t state, size_t expect_publisher_count)
+  {
+    rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+    pub_opts.qos = rmw_qos_profile_system_default;
+
+    rcl_ret_t ret =
+      rcl_action_client_configure_internal_service_introspection(
+        &action_client,
+        &node,
+        &clock,
+        action_typesupport,
+        pub_opts,
+        state);
+    ASSERT_TRUE(ret == RCL_RET_OK) << rcl_get_error_string().str;
+
+    // Check if internal service event publisher is not created by default
+    auto get_publisher_count = [this](const std::string & topic_name) -> size_t {
+        size_t publisher_count = 0;
+        rcl_ret_t ret = rcl_count_publishers(&this->node, topic_name.c_str(), &publisher_count);
+        EXPECT_TRUE(ret == RCL_RET_OK) << rcl_get_error_string().str;
+        rcl_reset_error();
+        if (ret != RCL_RET_OK) {
+          publisher_count = -1;
+        }
+        return publisher_count;
+      };
+
+    EXPECT_TRUE(
+      get_publisher_count(send_goal_service_event_topic_name) == expect_publisher_count);
+    EXPECT_TRUE(
+      get_publisher_count(cancel_goal_service_event_topic_name) == expect_publisher_count);
+    EXPECT_TRUE(
+      get_publisher_count(get_result_service_event_topic_name) == expect_publisher_count);
+  }
+
   const char * const action_name = "/test_action_client_name";
   rcl_action_client_options_t action_client_options;
   rcl_action_client_t invalid_action_client;
   rcl_action_client_t action_client;
+  const rosidl_action_type_support_t *action_typesupport;
+  rcl_clock_t clock;
+  std::string send_goal_service_event_topic_name;
+  std::string cancel_goal_service_event_topic_name;
+  std::string get_result_service_event_topic_name;
 };
 
 TEST_F(TestActionClientFixture, test_action_server_is_available) {
@@ -399,3 +450,38 @@ TEST_F(TestActionClientFixture, test_action_server_is_available_maybe_fail)
     rcl_reset_error();
   });
 }
+
+
+TEST_F(TestActionClientFixture, test_default_internal_services_introspection_status)
+{
+  // Check if internal service event publisher is not created by default
+  auto get_publisher_count = [this](const std::string & topic_name) -> size_t {
+      size_t publisher_count = 0;
+      rcl_ret_t ret = rcl_count_publishers(&this->node, topic_name.c_str(), &publisher_count);
+      EXPECT_TRUE(ret == RCL_RET_OK) << rcl_get_error_string().str;
+      rcl_reset_error();
+      if (ret != RCL_RET_OK) {
+        publisher_count = -1;
+      }
+      return publisher_count;
+    };
+
+  EXPECT_EQ(get_publisher_count(send_goal_service_event_topic_name), 0);
+  EXPECT_EQ(get_publisher_count(cancel_goal_service_event_topic_name), 0);
+  EXPECT_EQ(get_publisher_count(get_result_service_event_topic_name), 0);
+}
+
+TEST_F(TestActionClientFixture, test_set_internal_services_introspection_off)
+{
+  check_set_services_introspection(RCL_SERVICE_INTROSPECTION_OFF, 0);
+}
+
+TEST_F(TestActionClientFixture, test_set_internal_services_introspection_metadata)
+{
+  check_set_services_introspection(RCL_SERVICE_INTROSPECTION_METADATA, 1);
+}
+
+TEST_F(TestActionClientFixture, test_set_internal_services_introspection_contents)
+{
+  check_set_services_introspection(RCL_SERVICE_INTROSPECTION_CONTENTS, 1);
+}