Add new interfaces to enable introspection for action (#2743)

* Add new interfaces to enable intropsection for action

Signed-off-by: Barry Xu <barry.xu@sony.com>

* Correct some comments

Signed-off-by: Barry Xu <barry.xu@sony.com>

---------

Signed-off-by: Barry Xu <barry.xu@sony.com>

Author: Barry-Xu-2018

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"
 

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(

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(

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<rcl_node_t> node_handle{nullptr};
   std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
+  const rosidl_action_type_support_t * action_type_support_;
   rclcpp::Logger logger;
 
   using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;
@@ -801,4 +803,31 @@ ClientBase::execute(const std::shared_ptr<void> & 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

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<rcl_node_t> 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");
+  }
+}

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<ActionType>(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.
+}

rclcpp_action/test/test_server.cpp

@@ -1073,6 +1073,21 @@ class TestBasicServer : public TestServer
   std::shared_ptr<GoalHandle> 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)