feat(component_interface_utils): add interface

Signed-off-by: Takagi, Isamu <[email protected]>

common/component_interface_utils/include/component_interface_utils/rclcpp.hpp

@@ -16,11 +16,13 @@
 #define COMPONENT_INTERFACE_UTILS__RCLCPP_HPP_
 
 #include <component_interface_utils/rclcpp/create_interface.hpp>
+#include <component_interface_utils/rclcpp/interface.hpp>
 #include <component_interface_utils/rclcpp/service_client.hpp>
 #include <component_interface_utils/rclcpp/service_server.hpp>
 #include <component_interface_utils/rclcpp/topic_publisher.hpp>
 #include <component_interface_utils/rclcpp/topic_subscription.hpp>
 
+#include <memory>
 #include <optional>
 #include <utility>
 
@@ -43,38 +45,38 @@ class NodeAdaptor
 
 public:
   /// Constructor.
-  explicit NodeAdaptor(rclcpp::Node * node) : node_(node) {}
+  explicit NodeAdaptor(rclcpp::Node * node) { interface_ = std::make_shared<NodeInterface>(node); }
 
   /// Create a client wrapper for logging.
   template <class SharedPtrT>
   void init_cli(SharedPtrT & cli, CallbackGroup group = nullptr) const
   {
     using SpecT = typename SharedPtrT::element_type::SpecType;
-    cli = create_client_impl<SpecT>(node_, group);
+    cli = create_client_impl<SpecT>(interface_, group);
   }
 
   /// Create a service wrapper for logging.
   template <class SharedPtrT, class CallbackT>
   void init_srv(SharedPtrT & srv, CallbackT && callback, CallbackGroup group = nullptr) const
   {
     using SpecT = typename SharedPtrT::element_type::SpecType;
-    srv = create_service_impl<SpecT>(node_, std::forward<CallbackT>(callback), group);
+    srv = create_service_impl<SpecT>(interface_, std::forward<CallbackT>(callback), group);
   }
 
   /// Create a publisher using traits like services.
   template <class SharedPtrT>
   void init_pub(SharedPtrT & pub) const
   {
     using SpecT = typename SharedPtrT::element_type::SpecType;
-    pub = create_publisher_impl<SpecT>(node_);
+    pub = create_publisher_impl<SpecT>(interface_->node);
   }
 
   /// Create a subscription using traits like services.
   template <class SharedPtrT, class CallbackT>
   void init_sub(SharedPtrT & sub, CallbackT && callback) const
   {
     using SpecT = typename SharedPtrT::element_type::SpecType;
-    sub = create_subscription_impl<SpecT>(node_, std::forward<CallbackT>(callback));
+    sub = create_subscription_impl<SpecT>(interface_->node, std::forward<CallbackT>(callback));
   }
 
   /// Relay message.
@@ -119,7 +121,7 @@ class NodeAdaptor
 
 private:
   // Use a node pointer because shared_from_this cannot be used in constructor.
-  rclcpp::Node * node_;
+  NodeInterface::SharedPtr interface_;
 };
 
 }  // namespace component_interface_utils

common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp

@@ -15,6 +15,7 @@
 #ifndef COMPONENT_INTERFACE_UTILS__RCLCPP__CREATE_INTERFACE_HPP_
 #define COMPONENT_INTERFACE_UTILS__RCLCPP__CREATE_INTERFACE_HPP_
 
+#include <component_interface_utils/rclcpp/interface.hpp>
 #include <component_interface_utils/rclcpp/service_client.hpp>
 #include <component_interface_utils/rclcpp/service_server.hpp>
 #include <component_interface_utils/rclcpp/topic_publisher.hpp>
@@ -28,25 +29,24 @@ namespace component_interface_utils
 {
 
 /// Create a client wrapper for logging. This is a private implementation.
-template <class SpecT, class NodeT>
+template <class SpecT>
 typename Client<SpecT>::SharedPtr create_client_impl(
-  NodeT * node, rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  NodeInterface::SharedPtr interface, rclcpp::CallbackGroup::SharedPtr group = nullptr)
 {
   // This function is a wrapper for the following.
   // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L253-L265
-  auto client = node->template create_client<typename SpecT::Service>(
-    SpecT::name, rmw_qos_profile_services_default, group);
-  return Client<SpecT>::make_shared(client, node);
+  return Client<SpecT>::make_shared(interface, group);
 }
 
 /// Create a service wrapper for logging. This is a private implementation.
-template <class SpecT, class NodeT, class CallbackT>
+template <class SpecT, class CallbackT>
 typename Service<SpecT>::SharedPtr create_service_impl(
-  NodeT * node, CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  NodeInterface::SharedPtr interface, CallbackT && callback,
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
 {
   // This function is a wrapper for the following.
   // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L267-L281
-  return Service<SpecT>::make_shared(node, std::forward<CallbackT>(callback), group);
+  return Service<SpecT>::make_shared(interface, std::forward<CallbackT>(callback), group);
 }
 
 /// Create a publisher using traits like services. This is a private implementation.

common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp

@@ -0,0 +1,63 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef COMPONENT_INTERFACE_UTILS__RCLCPP__INTERFACE_HPP_
+#define COMPONENT_INTERFACE_UTILS__RCLCPP__INTERFACE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <tier4_system_msgs/msg/service_log.hpp>
+
+#include <memory>
+#include <string>
+
+namespace component_interface_utils
+{
+
+struct NodeInterface
+{
+  using SharedPtr = std::shared_ptr<NodeInterface>;
+  using ServiceLog = tier4_system_msgs::msg::ServiceLog;
+
+  explicit NodeInterface(rclcpp::Node * node)
+  {
+    this->node = node;
+    this->logger = node->create_publisher<ServiceLog>("/service_log", 10);
+
+    node_name_ = node->get_namespace();
+    if (node_name_.empty() || node_name_.back() != '/') {
+      node_name_ += "/";
+    }
+    node_name_ += node->get_name();
+  }
+
+  void log(ServiceLog::_type_type type, const std::string & name, const std::string & yaml)
+  {
+    ServiceLog msg;
+    msg.stamp = node->now();
+    msg.type = type;
+    msg.name = name;
+    msg.node = node_name_;
+    msg.yaml = yaml;
+    logger->publish(msg);
+  }
+
+  rclcpp::Node * node;
+  rclcpp::Publisher<ServiceLog>::SharedPtr logger;
+  std::string node_name_;
+};
+
+}  // namespace component_interface_utils
+
+#endif  // COMPONENT_INTERFACE_UTILS__RCLCPP__INTERFACE_HPP_

common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp

@@ -16,6 +16,7 @@
 #define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_CLIENT_HPP_
 
 #include <component_interface_utils/rclcpp/exceptions.hpp>
+#include <component_interface_utils/rclcpp/interface.hpp>
 #include <rclcpp/node.hpp>
 
 #include <tier4_system_msgs/msg/service_log.hpp>
@@ -38,28 +39,27 @@ class Client
   using ServiceLog = tier4_system_msgs::msg::ServiceLog;
 
   /// Constructor.
-  template <class NodeT>
-  Client(typename WrapType::SharedPtr client, NodeT * node)
+  Client(NodeInterface::SharedPtr interface, rclcpp::CallbackGroup::SharedPtr group)
+  : interface_(interface)
   {
-    client_ = client;  // to keep the reference count
-    pub_ = node->template create_publisher<ServiceLog>("/service_log", 10);
-    src_ = node->get_namespace() + std::string("/") + node->get_name();
+    client_ = interface->node->create_client<typename SpecT::Service>(
+      SpecT::name, rmw_qos_profile_services_default, group);
   }
 
   /// Send request.
   typename WrapType::SharedResponse call(
     const typename WrapType::SharedRequest request, std::optional<double> timeout = std::nullopt)
   {
     if (!client_->service_is_ready()) {
-      log(ServiceLog::ERROR_UNREADY);
+      interface_->log(ServiceLog::ERROR_UNREADY, SpecType::name, "");
       throw ServiceUnready(SpecT::name);
     }
 
     const auto future = this->async_send_request(request);
     if (timeout) {
       const auto duration = std::chrono::duration<double, std::ratio<1>>(timeout.value());
       if (future.wait_for(duration) != std::future_status::ready) {
-        log(ServiceLog::ERROR_TIMEOUT);
+        interface_->log(ServiceLog::ERROR_TIMEOUT, SpecType::name, "");
         throw ServiceTimeout(SpecT::name);
       }
     }
@@ -82,11 +82,11 @@ class Client
 #endif
 
     const auto wrapped = [this, callback](typename WrapType::SharedFuture future) {
-      log(ServiceLog::CLIENT_RESPONSE, to_yaml(*future.get()));
+      interface_->log(ServiceLog::CLIENT_RESPONSE, SpecType::name, to_yaml(*future.get()));
       callback(future);
     };
 
-    log(ServiceLog::CLIENT_REQUEST, to_yaml(*request));
+    interface_->log(ServiceLog::CLIENT_REQUEST, SpecType::name, to_yaml(*request));
 
 #ifdef ROS_DISTRO_GALACTIC
     return client_->async_send_request(request, wrapped);
@@ -98,20 +98,7 @@ class Client
 private:
   RCLCPP_DISABLE_COPY(Client)
   typename WrapType::SharedPtr client_;
-  rclcpp::Publisher<ServiceLog>::SharedPtr pub_;
-  std::string src_;
-
-  void log(ServiceLog::_type_type type, const std::string & yaml = "")
-  {
-    ServiceLog msg;
-    // msg.stamp =
-    msg.type = type;
-    msg.name = SpecT::name;
-    msg.node = src_;
-    // msg.guid =
-    msg.yaml = yaml;
-    pub_->publish(msg);
-  }
+  NodeInterface::SharedPtr interface_;
 };
 
 }  // namespace component_interface_utils

common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp

@@ -16,6 +16,7 @@
 #define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_SERVER_HPP_
 
 #include <component_interface_utils/rclcpp/exceptions.hpp>
+#include <component_interface_utils/rclcpp/interface.hpp>
 #include <rclcpp/node.hpp>
 
 #include <tier4_system_msgs/msg/service_log.hpp>
@@ -51,14 +52,14 @@ class Service
   using ServiceLog = tier4_system_msgs::msg::ServiceLog;
 
   /// Constructor.
-  template <class NodeT, class CallbackT>
-  Service(NodeT * node, CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  template <class CallbackT>
+  Service(
+    NodeInterface::SharedPtr interface, CallbackT && callback,
+    rclcpp::CallbackGroup::SharedPtr group)
+  : interface_(interface)
   {
-    service_ = node->template create_service<typename SpecT::Service>(
+    service_ = interface_->node->create_service<typename SpecT::Service>(
       SpecT::name, wrap(callback), rmw_qos_profile_services_default, group);
-
-    pub_ = node->template create_publisher<ServiceLog>("/service_log", 10);
-    src_ = node->get_namespace() + std::string("/") + node->get_name();
   }
 
   /// Create a service callback with logging added.
@@ -72,7 +73,7 @@ class Service
       using rosidl_generator_traits::to_yaml;
 #endif
       // If the response has status, convert it from the exception.
-      log(ServiceLog::SERVER_REQUEST, to_yaml(*request));
+      interface_->log(ServiceLog::SERVER_REQUEST, SpecType::name, to_yaml(*request));
       if constexpr (!has_status_type<typename SpecT::Service::Response>::value) {
         callback(request, response);
       } else {
@@ -82,28 +83,15 @@ class Service
           error.set(response->status);
         }
       }
-      log(ServiceLog::SERVER_RESPONSE, to_yaml(*response));
+      interface_->log(ServiceLog::SERVER_RESPONSE, SpecType::name, to_yaml(*response));
     };
     return wrapped;
   }
 
 private:
   RCLCPP_DISABLE_COPY(Service)
   typename WrapType::SharedPtr service_;
-  rclcpp::Publisher<tier4_system_msgs::msg::ServiceLog>::SharedPtr pub_;
-  std::string src_;
-
-  void log(ServiceLog::_type_type type, const std::string & yaml = "")
-  {
-    ServiceLog msg;
-    // msg.stamp =
-    msg.type = type;
-    msg.name = SpecT::name;
-    msg.node = src_;
-    // msg.guid =
-    msg.yaml = yaml;
-    pub_->publish(msg);
-  }
+  NodeInterface::SharedPtr interface_;
 };
 
 }  // namespace component_interface_utils