diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index ff821a4db2..97b3397012 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -30,11 +30,13 @@ #include "rcl/event_callback.h" #include "rclcpp/exceptions.hpp" +#include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/logger.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "rclcpp/waitable.hpp" diff --git a/rclcpp_action/include/rclcpp_action/client_base.hpp b/rclcpp_action/include/rclcpp_action/client_base.hpp index 9d10d66c7e..73cabfb9a0 100644 --- a/rclcpp_action/include/rclcpp_action/client_base.hpp +++ b/rclcpp_action/include/rclcpp_action/client_base.hpp @@ -177,6 +177,22 @@ class ClientBase : public rclcpp::Waitable // End Waitables API // ----------------- + /// Configure action client introspection. + /** + * \param[in] clock clock to use to generate introspection timestamps + * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher + * \param[in] introspection_state the state to set introspection to + * + * \throws std::invalid_argument if clock is nullptr + * \throws rclcpp::exceptions::throw_from_rcl_error if rcl error occurs. + */ + RCLCPP_ACTION_PUBLIC + void + configure_introspection( + rclcpp::Clock::SharedPtr clock, + const rclcpp::QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state); + protected: RCLCPP_ACTION_PUBLIC ClientBase( diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 00490fa163..6d41f351c0 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -28,9 +28,11 @@ #include "rcl_action/action_server.h" #include "rosidl_runtime_c/action_type_support_struct.h" #include "rosidl_typesupport_cpp/action_type_support.hpp" +#include "rclcpp/clock.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/waitable.hpp" #include "rclcpp_action/visibility_control.hpp" @@ -187,6 +189,22 @@ class ServerBase : public rclcpp::Waitable // End Waitables API // ----------------- + /// Configure action server introspection + /** + * \param[in] clock clock to use to generate introspection timestamps + * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher + * \param[in] introspection_state the state to set introspection to + * + * \throw std::invalid_argument if clock is nullptr + * \throw rclcpp::exceptions::throw_from_rcl_error if rcl error occurs. + */ + RCLCPP_ACTION_PUBLIC + void + configure_introspection( + rclcpp::Clock::SharedPtr clock, + const rclcpp::QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state); + protected: RCLCPP_ACTION_PUBLIC ServerBase( diff --git a/rclcpp_action/src/client_base.cpp b/rclcpp_action/src/client_base.cpp index 77cd5a39ef..34ce47d7bb 100644 --- a/rclcpp_action/src/client_base.cpp +++ b/rclcpp_action/src/client_base.cpp @@ -103,6 +103,7 @@ class ClientBaseImpl const rcl_action_client_options_t & client_options) : node_graph_(node_graph), node_handle(node_base->get_shared_rcl_node_handle()), + action_type_support_(type_support), logger(node_logging->get_logger().get_child("rclcpp_action")), random_bytes_generator(std::random_device{}()) { @@ -165,6 +166,7 @@ class ClientBaseImpl // node_handle must be destroyed after client_handle to prevent memory leak std::shared_ptr node_handle{nullptr}; std::shared_ptr client_handle{nullptr}; + const rosidl_action_type_support_t * action_type_support_; rclcpp::Logger logger; using ResponseCallback = std::function response)>; @@ -801,4 +803,31 @@ ClientBase::execute(const std::shared_ptr & data_in) }, data_ptr->data); } +void +ClientBase::configure_introspection( + rclcpp::Clock::SharedPtr clock, + const rclcpp::QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state) +{ + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile(); + + if (clock == nullptr) { + throw std::invalid_argument("clock is nullptr"); + } + + rcl_ret_t ret = rcl_action_client_configure_action_introspection( + pimpl_->client_handle.get(), + pimpl_->node_handle.get(), + clock->get_clock_handle(), + pimpl_->action_type_support_, + pub_opts, + introspection_state); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to configure action client introspection"); + } +} + } // namespace rclcpp_action diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index ed7f46f716..d3812f18ba 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -74,16 +74,25 @@ class ServerBaseImpl { public: ServerBaseImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + const rosidl_action_type_support_t * type_support, rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger ) - : clock_(clock), logger_(logger) + : node_handle_(node_base->get_shared_rcl_node_handle()), + action_type_support_(type_support), + clock_(clock), + logger_(logger) { } // Lock for action_server_ std::recursive_mutex action_server_reentrant_mutex_; + std::shared_ptr node_handle_; + + const rosidl_action_type_support_t * action_type_support_; + rclcpp::Clock::SharedPtr clock_; rclcpp::TimerBase::SharedPtr expire_timer_; @@ -124,7 +133,8 @@ ServerBase::ServerBase( const rcl_action_server_options_t & options ) : pimpl_(new ServerBaseImpl( - node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) + node_base, type_support, node_clock->get_clock(), + node_logging->get_logger().get_child("rclcpp_action"))) { auto deleter = [node_base](rcl_action_server_t * ptr) { @@ -929,3 +939,29 @@ ServerBase::get_timers() const { return {pimpl_->expire_timer_}; } + +void +ServerBase::configure_introspection( + rclcpp::Clock::SharedPtr clock, const rclcpp::QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state) +{ + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile(); + + if (clock == nullptr) { + throw std::invalid_argument("clock is nullptr"); + } + + rcl_ret_t ret = rcl_action_server_configure_action_introspection( + pimpl_->action_server_.get(), + pimpl_->node_handle_.get(), + clock->get_clock_handle(), + pimpl_->action_type_support_, + pub_opts, + introspection_state); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to configure action server introspection"); + } +} diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 743c10f7ab..744c1d1331 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -1035,3 +1035,20 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) rclcpp::exceptions::RCLError); } } + +TEST_F(TestClientAgainstServer, test_configure_introspection) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + + EXPECT_THROW( + action_client->configure_introspection( + nullptr, rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS), + std::invalid_argument); + + EXPECT_NO_THROW( + action_client->configure_introspection( + client_node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS)); + + // No method was found to make rcl_action_client_configure_action_introspection return + // a value other than RCL_RET_OK. mocking_utils::patch_and_return does not work for this function. +} diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 91ee931958..cc0aa015d1 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1073,6 +1073,21 @@ class TestBasicServer : public TestServer std::shared_ptr goal_handle_; }; +TEST_F(TestBasicServer, test_configure_introspection) +{ + EXPECT_THROW( + action_server_->configure_introspection( + nullptr, rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS), + std::invalid_argument); + + EXPECT_NO_THROW( + action_server_->configure_introspection( + node_->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS)); + + // No method was found to make rcl_action_server_configure_action_introspection return + // a value other than RCL_RET_OK. mocking_utils::patch_and_return does not work for this function. +} + class TestGoalRequestServer : public TestBasicServer {}; TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal)