diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h
index e3a447a77..46566efdf 100644
--- a/rcl/include/rcl/client.h
+++ b/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
diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h
index 330cf17af..3cae55ce1 100644
--- a/rcl/include/rcl/service.h
+++ b/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
diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt
index ed9f7a353..5526d2c8d 100644
--- a/rcl_action/CMakeLists.txt
+++ b/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}
diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h
index e49966c4d..69ab136d2 100644
--- a/rcl_action/include/rcl_action/action_client.h
+++ b/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.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | Maybe [1]
+ * Lock-Free | Maybe [1]
+ * [1] rmw implementation defined
+ *
+ * \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_action_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
diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h
index 1ea48a975..8bbd137da 100644
--- a/rcl_action/include/rcl_action/action_server.h
+++ b/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.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | Maybe [1]
+ * Lock-Free | Maybe [1]
+ * [1] rmw implementation defined
+ *
+ * \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_action_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
diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c
index 07c40ba0a..992d52850 100644
--- a/rcl_action/src/rcl_action/action_client.c
+++ b/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) \
+ { \
+ return RCL_RET_ERROR; \
+ }
+
+
+rcl_ret_t
+rcl_action_client_configure_action_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);
+
+ CLIENT_CONFIGURE_SERVICE_INTROSPECTION(goal, introspection_state);
+ CLIENT_CONFIGURE_SERVICE_INTROSPECTION(cancel, introspection_state);
+ CLIENT_CONFIGURE_SERVICE_INTROSPECTION(result, introspection_state);
+ return RCL_RET_OK;
+}
+
#ifdef __cplusplus
}
#endif
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index b668a9058..dc510f783 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -1240,6 +1240,40 @@ 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) \
+ { \
+ return RCL_RET_ERROR; \
+ }
+
+rcl_ret_t
+rcl_action_server_configure_action_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);
+
+ SERVER_CONFIGURE_SERVICE_INTROSPECTION(goal, introspection_state);
+ SERVER_CONFIGURE_SERVICE_INTROSPECTION(cancel, introspection_state);
+ SERVER_CONFIGURE_SERVICE_INTROSPECTION(result, introspection_state);
+ return RCL_RET_OK;
+}
+
#ifdef __cplusplus
}
#endif
diff --git a/rcl_action/test/rcl_action/test_action_client.cpp b/rcl_action/test/rcl_action/test_action_client.cpp
index b46f40abf..d166c5af0 100644
--- a/rcl_action/test/rcl_action/test_action_client.cpp
+++ b/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_action_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);
+}
diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp
index e2ae3e58c..c6e89023b 100644
--- a/rcl_action/test/rcl_action/test_action_communication.cpp
+++ b/rcl_action/test/rcl_action/test_action_communication.cpp
@@ -20,10 +20,12 @@
#include "rcl_action/action_client.h"
#include "rcl_action/action_server.h"
+#include "rcl_action/action_server_impl.h"
#include "rcl_action/wait.h"
#include "rcl/error_handling.h"
#include "rcl/rcl.h"
+#include "rcl/service_introspection.h"
#include "rosidl_runtime_c/primitives_sequence_functions.h"
@@ -54,8 +56,7 @@ class TestActionCommunication : public ::testing::Test
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(
- test_msgs, Fibonacci);
+ ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
const char * action_name = "test_action_commmunication_name";
const rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
this->action_server = rcl_action_get_zero_initialized_server();
@@ -157,6 +158,7 @@ class TestActionCommunication : public ::testing::Test
}
}
+ const rosidl_action_type_support_t * ts;
rcl_action_client_t action_client;
rcl_action_server_t action_server;
rcl_context_t context;
@@ -1193,3 +1195,539 @@ TEST_F(TestActionCommunication, test_valid_feedback_comm_maybe_fail)
test_msgs__action__Fibonacci_FeedbackMessage__fini(&outgoing_feedback);
});
}
+
+class TestActionIntrospection : public TestActionCommunication
+{
+public:
+ void SetUp() override
+ {
+ TestActionCommunication::SetUp();
+ send_goal_service_event_topic_name = std::string(action_server.impl->remapped_action_name) +
+ "/_action/send_goal" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
+ cancel_goal_service_event_topic_name = std::string(action_server.impl->remapped_action_name) +
+ "/_action/cancel_goal" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
+ get_result_service_event_topic_name = std::string(action_server.impl->remapped_action_name) +
+ "/_action/get_result" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
+
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+ send_goal_service_event_sub_ptr = new rcl_subscription_t;
+ *send_goal_service_event_sub_ptr = rcl_get_zero_initialized_subscription();
+ rcl_ret_t ret = rcl_subscription_init(
+ send_goal_service_event_sub_ptr, &node, ts->goal_service_type_support->event_typesupport,
+ send_goal_service_event_topic_name.c_str(), &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+ cancel_goal_service_event_sub_ptr = new rcl_subscription_t;
+ *cancel_goal_service_event_sub_ptr = rcl_get_zero_initialized_subscription();
+ ret = rcl_subscription_init(
+ cancel_goal_service_event_sub_ptr, &node, ts->cancel_service_type_support->event_typesupport,
+ cancel_goal_service_event_topic_name.c_str(), &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+ get_result_service_event_sub_ptr = new rcl_subscription_t;
+ *get_result_service_event_sub_ptr = rcl_get_zero_initialized_subscription();
+ ret = rcl_subscription_init(
+ get_result_service_event_sub_ptr, &node, ts->result_service_type_support->event_typesupport,
+ get_result_service_event_topic_name.c_str(), &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ }
+
+ void TearDown() override
+ {
+ rcl_ret_t ret = rcl_subscription_fini(send_goal_service_event_sub_ptr, &node);
+ delete send_goal_service_event_sub_ptr;
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+ ret = rcl_subscription_fini(cancel_goal_service_event_sub_ptr, &node);
+ delete cancel_goal_service_event_sub_ptr;
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+ ret = rcl_subscription_fini(get_result_service_event_sub_ptr, &node);
+ delete get_result_service_event_sub_ptr;
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+
+ TestActionCommunication::TearDown();
+ }
+
+ void enable_action_server_introspection()
+ {
+ 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_server_configure_action_introspection(
+ &action_server,
+ &node,
+ &clock,
+ ts,
+ pub_opts,
+ RCL_SERVICE_INTROSPECTION_CONTENTS);
+ ASSERT_TRUE(ret == RCL_RET_OK) << rcl_get_error_string().str;
+ }
+
+ void enable_action_client_introspection()
+ {
+ 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_action_introspection(
+ &action_client,
+ &node,
+ &clock,
+ ts,
+ pub_opts,
+ RCL_SERVICE_INTROSPECTION_CONTENTS);
+ ASSERT_TRUE(ret == RCL_RET_OK) << rcl_get_error_string().str;
+ }
+
+ bool wait_for_send_goal_service_event_subscription_to_be_ready(
+ size_t max_tries, int64_t period_ms)
+ {
+ return wait_for_subscription_to_be_ready(
+ send_goal_service_event_sub_ptr,
+ max_tries,
+ period_ms);
+ }
+
+ bool wait_for_cancel_goal_service_event_subscription_to_be_ready(
+ size_t max_tries, int64_t period_ms)
+ {
+ return wait_for_subscription_to_be_ready(
+ cancel_goal_service_event_sub_ptr,
+ max_tries,
+ period_ms);
+ }
+
+ bool wait_for_get_result_service_event_subscription_to_be_ready(
+ size_t max_tries, int64_t period_ms)
+ {
+ return wait_for_subscription_to_be_ready(
+ get_result_service_event_sub_ptr,
+ max_tries,
+ period_ms);
+ }
+
+ std::string send_goal_service_event_topic_name;
+ std::string cancel_goal_service_event_topic_name;
+ std::string get_result_service_event_topic_name;
+ rcl_subscription_t * send_goal_service_event_sub_ptr;
+ rcl_subscription_t * cancel_goal_service_event_sub_ptr;
+ rcl_subscription_t * get_result_service_event_sub_ptr;
+
+private:
+ bool wait_for_subscription_to_be_ready(
+ rcl_subscription_t * subscription,
+ size_t max_tries,
+ int64_t period_ms)
+ {
+ rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+ rcl_ret_t ret =
+ rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &context, rcl_get_default_allocator());
+ if (ret != RCL_RET_OK) {
+ RCUTILS_LOG_ERROR_NAMED(
+ ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str);
+ return false;
+ }
+ OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
+ {
+ if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
+ RCUTILS_LOG_ERROR_NAMED(
+ ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str);
+ throw std::runtime_error("error waiting for service to be ready");
+ }
+ });
+ size_t iteration = 0;
+ while (iteration < max_tries) {
+ ++iteration;
+ if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
+ RCUTILS_LOG_ERROR_NAMED(
+ ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str);
+ return false;
+ }
+ if (rcl_wait_set_add_subscription(&wait_set, subscription, NULL) != RCL_RET_OK) {
+ RCUTILS_LOG_ERROR_NAMED(
+ ROS_PACKAGE_NAME,
+ "Error in rcl_wait_set_add_subscription: %s", rcl_get_error_string().str);
+ return false;
+ }
+ ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
+ if (ret == RCL_RET_TIMEOUT) {
+ continue;
+ }
+ if (ret != RCL_RET_OK) {
+ RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str);
+ return false;
+ }
+ for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) {
+ if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) {
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+};
+
+// The following test primarily verifies that when the action server/action client sets
+// introspection to contents, all three internal services successfully set introspection
+// to contents.
+
+TEST_F(TestActionIntrospection, test_action_server_valid_send_goal_service_event)
+{
+ // Enable introspection for action server
+ enable_action_server_introspection();
+
+ test_msgs__action__Fibonacci_SendGoal_Request outgoing_goal_request;
+ test_msgs__action__Fibonacci_SendGoal_Request incoming_goal_request;
+ test_msgs__action__Fibonacci_SendGoal_Request__init(&outgoing_goal_request);
+ test_msgs__action__Fibonacci_SendGoal_Request__init(&incoming_goal_request);
+
+ // Initialize goal request
+ init_test_uuid0(outgoing_goal_request.goal_id.uuid);
+ outgoing_goal_request.goal.order = 10;
+
+ // Send goal request with valid arguments
+ int64_t sequence_number;
+ rcl_ret_t ret = rcl_action_send_goal_request(
+ &action_client, &outgoing_goal_request, &sequence_number);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_wait_set_add_action_server(&wait_set, &action_server, NULL);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_wait(&wait_set, RCL_S_TO_NS(10));
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_server_wait_set_get_entities_ready(
+ &wait_set,
+ &action_server,
+ &is_goal_request_ready,
+ &is_cancel_request_ready,
+ &is_result_request_ready,
+ &is_goal_expired);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ EXPECT_TRUE(is_goal_request_ready) << rcl_get_error_string().str;
+ EXPECT_FALSE(is_cancel_request_ready) << rcl_get_error_string().str;
+ EXPECT_FALSE(is_result_request_ready) << rcl_get_error_string().str;
+
+ // Take goal request with valid arguments
+ rmw_request_id_t request_header;
+ ret = rcl_action_take_goal_request(&action_server, &request_header, &incoming_goal_request);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ // Check that send goal service event was received correctly
+ ASSERT_TRUE(wait_for_send_goal_service_event_subscription_to_be_ready(5, 100));
+ test_msgs__action__Fibonacci_SendGoal_Event send_goal_event;
+ test_msgs__action__Fibonacci_SendGoal_Event__init(&send_goal_event);
+ rmw_message_info_t message_info = rmw_get_zero_initialized_message_info();
+ ret = rcl_take(send_goal_service_event_sub_ptr, &send_goal_event, &message_info, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_RECEIVED, send_goal_event.info.event_type);
+ // Check that the request part of the service event contains content
+ ASSERT_GT(send_goal_event.request.size, 0);
+
+ test_msgs__action__Fibonacci_SendGoal_Request__fini(&outgoing_goal_request);
+ test_msgs__action__Fibonacci_SendGoal_Request__fini(&incoming_goal_request);
+}
+
+TEST_F(TestActionIntrospection, test_action_server_valid_cancel_goal_service_event)
+{
+ // Enable introspection for action server
+ enable_action_server_introspection();
+
+ action_msgs__srv__CancelGoal_Request outgoing_cancel_request;
+ action_msgs__srv__CancelGoal_Request incoming_cancel_request;
+ action_msgs__srv__CancelGoal_Request__init(&outgoing_cancel_request);
+ action_msgs__srv__CancelGoal_Request__init(&incoming_cancel_request);
+
+ // Initialize cancel request
+ init_test_uuid0(outgoing_cancel_request.goal_info.goal_id.uuid);
+ outgoing_cancel_request.goal_info.stamp.sec = 321;
+ outgoing_cancel_request.goal_info.stamp.nanosec = 987654u;
+
+ // Send cancel request with valid arguments
+ int64_t sequence_number = 1324;
+ rcl_ret_t ret = rcl_action_send_cancel_request(
+ &action_client, &outgoing_cancel_request, &sequence_number);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_wait_set_add_action_server(&wait_set, &action_server, NULL);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_wait(&wait_set, RCL_S_TO_NS(10));
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_server_wait_set_get_entities_ready(
+ &wait_set,
+ &action_server,
+ &is_goal_request_ready,
+ &is_cancel_request_ready,
+ &is_result_request_ready,
+ &is_goal_expired);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ EXPECT_TRUE(is_cancel_request_ready);
+ EXPECT_FALSE(is_goal_request_ready);
+ EXPECT_FALSE(is_result_request_ready);
+
+ // Take cancel request with valid arguments
+ rmw_request_id_t request_header;
+ ret = rcl_action_take_cancel_request(
+ &action_server, &request_header, &incoming_cancel_request);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ // Check that cancel goal service event was received correctly
+ ASSERT_TRUE(wait_for_cancel_goal_service_event_subscription_to_be_ready(5, 100));
+ action_msgs__srv__CancelGoal_Event cancel_event;
+ action_msgs__srv__CancelGoal_Event__init(&cancel_event);
+ rmw_message_info_t message_info = rmw_get_zero_initialized_message_info();
+ ret = rcl_take(cancel_goal_service_event_sub_ptr, &cancel_event, &message_info, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_RECEIVED, cancel_event.info.event_type);
+ // Check that the request part of the service event contains content.
+ ASSERT_GT(cancel_event.request.size, 0);
+
+ action_msgs__srv__CancelGoal_Request__fini(&incoming_cancel_request);
+ action_msgs__srv__CancelGoal_Request__fini(&outgoing_cancel_request);
+}
+
+TEST_F(TestActionIntrospection, test_action_server_valid_get_result_service_event)
+{
+ // Enable introspection for action server
+ enable_action_server_introspection();
+
+ test_msgs__action__Fibonacci_GetResult_Request outgoing_result_request;
+ test_msgs__action__Fibonacci_GetResult_Request incoming_result_request;
+ test_msgs__action__Fibonacci_GetResult_Request__init(&outgoing_result_request);
+ test_msgs__action__Fibonacci_GetResult_Request__init(&incoming_result_request);
+
+ // Initialize result request
+ init_test_uuid0(outgoing_result_request.goal_id.uuid);
+
+ // Send result request with valid arguments
+ int64_t sequence_number;
+ rcl_ret_t ret = rcl_action_send_result_request(
+ &this->action_client, &outgoing_result_request, &sequence_number);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_server_wait_set_get_entities_ready(
+ &this->wait_set,
+ &this->action_server,
+ &this->is_goal_request_ready,
+ &this->is_cancel_request_ready,
+ &this->is_result_request_ready,
+ &this->is_goal_expired);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ EXPECT_TRUE(this->is_result_request_ready);
+ EXPECT_FALSE(this->is_cancel_request_ready);
+ EXPECT_FALSE(this->is_goal_request_ready);
+
+ // Take result request with valid arguments
+ rmw_request_id_t request_header;
+ ret = rcl_action_take_result_request(
+ &this->action_server, &request_header, &incoming_result_request);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ // Check that get result service event was received correctly
+ ASSERT_TRUE(wait_for_get_result_service_event_subscription_to_be_ready(5, 100));
+ test_msgs__action__Fibonacci_GetResult_Event get_result_event;
+ test_msgs__action__Fibonacci_GetResult_Event__init(&get_result_event);
+ rmw_message_info_t message_info = rmw_get_zero_initialized_message_info();
+ ret = rcl_take(get_result_service_event_sub_ptr, &get_result_event, &message_info, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_RECEIVED,
+ get_result_event.info.event_type);
+ // Check that the request part of the service event contains content
+ ASSERT_GT(get_result_event.request.size, 0);
+
+ test_msgs__action__Fibonacci_GetResult_Request__fini(&incoming_result_request);
+ test_msgs__action__Fibonacci_GetResult_Request__fini(&outgoing_result_request);
+}
+
+TEST_F(TestActionIntrospection, test_action_client_valid_send_goal_service_event)
+{
+ // Enable introspection for action client
+ enable_action_client_introspection();
+
+ test_msgs__action__Fibonacci_SendGoal_Request outgoing_goal_request;
+ test_msgs__action__Fibonacci_SendGoal_Request incoming_goal_request;
+ test_msgs__action__Fibonacci_SendGoal_Request__init(&outgoing_goal_request);
+ test_msgs__action__Fibonacci_SendGoal_Request__init(&incoming_goal_request);
+
+ // Initialize goal request
+ init_test_uuid0(outgoing_goal_request.goal_id.uuid);
+ outgoing_goal_request.goal.order = 10;
+
+ // Send goal request with valid arguments
+ int64_t sequence_number;
+ rcl_ret_t ret = rcl_action_send_goal_request(
+ &action_client, &outgoing_goal_request, &sequence_number);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_wait_set_add_action_server(&wait_set, &action_server, NULL);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_wait(&wait_set, RCL_S_TO_NS(10));
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_server_wait_set_get_entities_ready(
+ &wait_set,
+ &action_server,
+ &is_goal_request_ready,
+ &is_cancel_request_ready,
+ &is_result_request_ready,
+ &is_goal_expired);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ EXPECT_TRUE(is_goal_request_ready) << rcl_get_error_string().str;
+ EXPECT_FALSE(is_cancel_request_ready) << rcl_get_error_string().str;
+ EXPECT_FALSE(is_result_request_ready) << rcl_get_error_string().str;
+
+ // Take goal request with valid arguments
+ rmw_request_id_t request_header;
+ ret = rcl_action_take_goal_request(&action_server, &request_header, &incoming_goal_request);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ // Check that send goal service event was received correctly
+ ASSERT_TRUE(wait_for_send_goal_service_event_subscription_to_be_ready(5, 100));
+ test_msgs__action__Fibonacci_SendGoal_Event send_goal_event;
+ test_msgs__action__Fibonacci_SendGoal_Event__init(&send_goal_event);
+ rmw_message_info_t message_info = rmw_get_zero_initialized_message_info();
+ ret = rcl_take(send_goal_service_event_sub_ptr, &send_goal_event, &message_info, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_SENT, send_goal_event.info.event_type);
+ // Check that the request part of the service event contains content.
+ ASSERT_GT(send_goal_event.request.size, 0);
+
+ test_msgs__action__Fibonacci_SendGoal_Request__fini(&outgoing_goal_request);
+ test_msgs__action__Fibonacci_SendGoal_Request__fini(&incoming_goal_request);
+}
+
+TEST_F(TestActionIntrospection, test_action_client_valid_cancel_goal_service_event)
+{
+ // Enable introspection for action client
+ enable_action_client_introspection();
+
+ action_msgs__srv__CancelGoal_Request outgoing_cancel_request;
+ action_msgs__srv__CancelGoal_Request incoming_cancel_request;
+ action_msgs__srv__CancelGoal_Request__init(&outgoing_cancel_request);
+ action_msgs__srv__CancelGoal_Request__init(&incoming_cancel_request);
+
+ // Initialize cancel request
+ init_test_uuid0(outgoing_cancel_request.goal_info.goal_id.uuid);
+ outgoing_cancel_request.goal_info.stamp.sec = 321;
+ outgoing_cancel_request.goal_info.stamp.nanosec = 987654u;
+
+ // Send cancel request with valid arguments
+ int64_t sequence_number = 1324;
+ rcl_ret_t ret = rcl_action_send_cancel_request(
+ &action_client, &outgoing_cancel_request, &sequence_number);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_wait_set_add_action_server(&wait_set, &action_server, NULL);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_wait(&wait_set, RCL_S_TO_NS(10));
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_server_wait_set_get_entities_ready(
+ &wait_set,
+ &action_server,
+ &is_goal_request_ready,
+ &is_cancel_request_ready,
+ &is_result_request_ready,
+ &is_goal_expired);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ EXPECT_TRUE(is_cancel_request_ready);
+ EXPECT_FALSE(is_goal_request_ready);
+ EXPECT_FALSE(is_result_request_ready);
+
+ // Take cancel request with valid arguments
+ rmw_request_id_t request_header;
+ ret = rcl_action_take_cancel_request(
+ &action_server, &request_header, &incoming_cancel_request);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ // Check that cancel goal service event was received correctly
+ ASSERT_TRUE(wait_for_cancel_goal_service_event_subscription_to_be_ready(5, 100));
+ action_msgs__srv__CancelGoal_Event cancel_event;
+ action_msgs__srv__CancelGoal_Event__init(&cancel_event);
+ rmw_message_info_t message_info = rmw_get_zero_initialized_message_info();
+ ret = rcl_take(cancel_goal_service_event_sub_ptr, &cancel_event, &message_info, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_SENT, cancel_event.info.event_type);
+ // Check that the request part of the service event contains content.
+ ASSERT_GT(cancel_event.request.size, 0);
+
+ action_msgs__srv__CancelGoal_Request__fini(&incoming_cancel_request);
+ action_msgs__srv__CancelGoal_Request__fini(&outgoing_cancel_request);
+}
+
+TEST_F(TestActionIntrospection, test_action_client_valid_get_result_service_event)
+{
+ // Enable introspection for action client
+ enable_action_client_introspection();
+
+ test_msgs__action__Fibonacci_GetResult_Request outgoing_result_request;
+ test_msgs__action__Fibonacci_GetResult_Request incoming_result_request;
+ test_msgs__action__Fibonacci_GetResult_Request__init(&outgoing_result_request);
+ test_msgs__action__Fibonacci_GetResult_Request__init(&incoming_result_request);
+
+ // Initialize result request
+ init_test_uuid0(outgoing_result_request.goal_id.uuid);
+
+ // Send result request with valid arguments
+ int64_t sequence_number;
+ rcl_ret_t ret = rcl_action_send_result_request(
+ &this->action_client, &outgoing_result_request, &sequence_number);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL);
+ ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ ret = rcl_action_server_wait_set_get_entities_ready(
+ &this->wait_set,
+ &this->action_server,
+ &this->is_goal_request_ready,
+ &this->is_cancel_request_ready,
+ &this->is_result_request_ready,
+ &this->is_goal_expired);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ EXPECT_TRUE(this->is_result_request_ready);
+ EXPECT_FALSE(this->is_cancel_request_ready);
+ EXPECT_FALSE(this->is_goal_request_ready);
+
+ // Take result request with valid arguments
+ rmw_request_id_t request_header;
+ ret = rcl_action_take_result_request(
+ &this->action_server, &request_header, &incoming_result_request);
+ EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ // Check that get result service event was received correctly
+ ASSERT_TRUE(wait_for_get_result_service_event_subscription_to_be_ready(5, 100));
+ test_msgs__action__Fibonacci_GetResult_Event get_result_event;
+ test_msgs__action__Fibonacci_GetResult_Event__init(&get_result_event);
+ rmw_message_info_t message_info = rmw_get_zero_initialized_message_info();
+ ret = rcl_take(get_result_service_event_sub_ptr, &get_result_event, &message_info, nullptr);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
+ ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_SENT, get_result_event.info.event_type);
+ // Check that the request part of the service event contains content
+ ASSERT_GT(get_result_event.request.size, 0);
+
+ test_msgs__action__Fibonacci_GetResult_Request__fini(&incoming_result_request);
+ test_msgs__action__Fibonacci_GetResult_Request__fini(&outgoing_result_request);
+}
diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp
index 530a535d6..76e4ff347 100644
--- a/rcl_action/test/rcl_action/test_action_server.cpp
+++ b/rcl_action/test/rcl_action/test_action_server.cpp
@@ -26,7 +26,9 @@
#include "rcl_action/action_server_impl.h"
#include "rcl/error_handling.h"
+#include "rcl/graph.h"
#include "rcl/rcl.h"
+#include "rcl/service_introspection.h"
#include "test_msgs/action/fibonacci.h"
@@ -205,14 +207,20 @@ class TestActionServer : public ::testing::Test
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_clock_init(RCL_ROS_TIME, &this->clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
- const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(
- test_msgs, Fibonacci);
+ ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
const rcl_action_server_options_t options = rcl_action_server_get_default_options();
const char * action_name = "test_action_server_name";
this->action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_server_init(
&this->action_server, &this->node, &this->clock, ts, action_name, &options);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
+
+ send_goal_service_event_topic_name = std::string(action_server.impl->remapped_action_name) +
+ "/_action/send_goal" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
+ cancel_goal_service_event_topic_name = std::string(action_server.impl->remapped_action_name) +
+ "/_action/cancel_goal" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
+ get_result_service_event_topic_name = std::string(action_server.impl->remapped_action_name) +
+ "/_action/get_result" + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX;
}
void TearDown() override
@@ -244,10 +252,50 @@ class TestActionServer : public ::testing::Test
}
}
+ 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_server_configure_action_introspection(
+ &action_server,
+ &node,
+ &clock,
+ ts,
+ 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);
+ }
+
rcl_action_server_t action_server;
rcl_context_t context;
rcl_node_t node;
rcl_clock_t clock;
+ const rosidl_action_type_support_t *ts;
+ std::string send_goal_service_event_topic_name;
+ std::string cancel_goal_service_event_topic_name;
+ std::string get_result_service_event_topic_name;
}; // class TestActionServer
TEST_F(TestActionServer, test_action_server_is_valid)
@@ -804,6 +852,56 @@ TEST_F(TestActionServer, test_action_server_get_options)
EXPECT_NE(options, nullptr) << rcl_get_error_string().str;
}
+TEST_F(TestActionServer, test_default_internal_services_introspection_status)
+{
+ // Check valid action server
+ bool is_valid = rcl_action_server_is_valid(&this->action_server);
+ ASSERT_TRUE(is_valid) << 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_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(TestActionServer, test_set_internal_services_introspection_off)
+{
+ // Check valid action server
+ bool is_valid = rcl_action_server_is_valid(&this->action_server);
+ ASSERT_TRUE(is_valid) << rcl_get_error_string().str;
+
+ check_set_services_introspection(RCL_SERVICE_INTROSPECTION_OFF, 0);
+}
+
+TEST_F(TestActionServer, test_set_internal_services_introspection_metadata)
+{
+ // Check valid action server
+ bool is_valid = rcl_action_server_is_valid(&this->action_server);
+ ASSERT_TRUE(is_valid) << rcl_get_error_string().str;
+
+ check_set_services_introspection(RCL_SERVICE_INTROSPECTION_METADATA, 1);
+}
+
+TEST_F(TestActionServer, test_set_internal_services_introspection_contents)
+{
+ // Check valid action server
+ bool is_valid = rcl_action_server_is_valid(&this->action_server);
+ ASSERT_TRUE(is_valid) << rcl_get_error_string().str;
+
+ check_set_services_introspection(RCL_SERVICE_INTROSPECTION_CONTENTS, 1);
+}
+
class TestActionServerCancelPolicy : public TestActionServer
{
protected: