feat(component_interface_utils): add interface classes (tier4#899)

* feat(component_interface_utils): add interface classes

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(default_ad_api): apply the changes of interface utils

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix(component_interface_utils): remove old comment

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix(component_interface_utils): add client log

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix(component_interface_utils): remove unimplemented message

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* docs(component_interface_utils): add design policy

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* docs(component_interface_utils): add comment

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

common/component_interface_utils/README.md

@@ -24,3 +24,9 @@ This is a utility package that provides the following features:
    component_interface_utils::Service<SampleService>::SharedPtr srv_;
    srv_ = component_interface_utils::create_service<SampleService>(node, ...);
    ```
+
+## Design
+
+This package provides the wrappers for the interface classes of rclcpp.
+The wrappers limit the usage of the original class to enforce the processing recommended by the component interface.
+Do not inherit the class of rclcpp, and forward or wrap the member function that is allowed to be used.

common/component_interface_utils/include/component_interface_utils/rclcpp.hpp

@@ -16,6 +16,62 @@
 #define COMPONENT_INTERFACE_UTILS__RCLCPP_HPP_
 
 #include <component_interface_utils/rclcpp/create_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 <utility>
+
+namespace component_interface_utils
+{
+
+class NodeAdaptor
+{
+private:
+  using CallbackGroup = rclcpp::CallbackGroup::SharedPtr;
+
+public:
+  /// Constructor.
+  explicit NodeAdaptor(rclcpp::Node * node) : node_(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);
+  }
+
+  /// 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);
+  }
+
+  /// 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_);
+  }
+
+  /// 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));
+  }
+
+private:
+  // Use a node pointer because shared_from_this cannot be used in constructor.
+  rclcpp::Node * node_;
+};
+
+}  // namespace component_interface_utils
 
 #endif  // COMPONENT_INTERFACE_UTILS__RCLCPP_HPP_

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

@@ -15,20 +15,35 @@
 #ifndef COMPONENT_INTERFACE_UTILS__RCLCPP__CREATE_INTERFACE_HPP_
 #define COMPONENT_INTERFACE_UTILS__RCLCPP__CREATE_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 <component_interface_utils/specs.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <utility>
 
 namespace component_interface_utils
 {
 
+/// Create a client wrapper for logging. This is a private implementation.
+template <class SpecT, class NodeT>
+typename Client<SpecT>::SharedPtr create_client_impl(
+  NodeT * node, 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->get_logger());
+}
+
 /// Create a service wrapper for logging. This is a private implementation.
 template <class SpecT, class NodeT, class CallbackT>
 typename Service<SpecT>::SharedPtr create_service_impl(
   NodeT * node, CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr)
 {
-  // Use a node pointer because shared_from_this cannot be used in constructor.
   // This function is a wrapper for the following.
   // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L267-L281
   auto wrapped = Service<SpecT>::wrap(callback, node->get_logger());
@@ -37,23 +52,27 @@ typename Service<SpecT>::SharedPtr create_service_impl(
   return Service<SpecT>::make_shared(service);
 }
 
-/// Create a service wrapper for logging. This is for lambda or bound function.
-template <class SpecT, class NodeT, class CallbackT>
-typename Service<SpecT>::SharedPtr create_service(
-  NodeT * node, CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr)
+/// Create a publisher using traits like services. This is a private implementation.
+template <class SpecT, class NodeT>
+typename Publisher<SpecT>::SharedPtr create_publisher_impl(NodeT * node)
 {
-  return create_service_impl<SpecT>(node, std::forward<CallbackT>(callback), group);
+  // This function is a wrapper for the following.
+  // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L167-L205
+  auto publisher =
+    node->template create_publisher<typename SpecT::Message>(SpecT::name, get_qos<SpecT>());
+  return Publisher<SpecT>::make_shared(publisher);
 }
 
-/// Create a service wrapper for logging. This is for member function of node.
-template <class SpecT, class NodeT>
-typename Service<SpecT>::SharedPtr create_service(
-  NodeT * node, typename Service<SpecT>::template CallbackType<NodeT> callback,
-  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+/// Create a subscription using traits like services. This is a private implementation.
+template <class SpecT, class NodeT, class CallbackT>
+typename Subscription<SpecT>::SharedPtr create_subscription_impl(
+  NodeT * node, CallbackT && callback)
 {
-  using std::placeholders::_1;
-  using std::placeholders::_2;
-  return create_service_impl<SpecT>(node, std::bind(callback, node, _1, _2), group);
+  // This function is a wrapper for the following.
+  // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L207-L238
+  auto subscription = node->template create_subscription<typename SpecT::Message>(
+    SpecT::name, get_qos<SpecT>(), std::forward<CallbackT>(callback));
+  return Subscription<SpecT>::make_shared(subscription);
 }
 
 }  // namespace component_interface_utils

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

@@ -0,0 +1,76 @@
+// 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__SERVICE_CLIENT_HPP_
+#define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_CLIENT_HPP_
+
+#include <rclcpp/client.hpp>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
+
+#include <utility>
+
+namespace component_interface_utils
+{
+
+/// The wrapper class of rclcpp::Client for logging.
+template <class SpecT>
+class Client
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Client)
+  using SpecType = SpecT;
+  using WrapType = rclcpp::Client<typename SpecT::Service>;
+
+  /// Constructor.
+  explicit Client(typename WrapType::SharedPtr client, const rclcpp::Logger & logger)
+  : logger_(logger)
+  {
+    client_ = client;  // to keep the reference count
+  }
+
+  /// Send request.
+  typename WrapType::SharedFuture async_send_request(typename WrapType::SharedRequest request)
+  {
+    const auto callback = [this](typename WrapType::SharedFuture future) {};
+    return this->async_send_request(request, callback);
+  }
+
+  /// Send request.
+  template <class CallbackT>
+  typename WrapType::SharedFuture async_send_request(
+    typename WrapType::SharedRequest request, CallbackT && callback)
+  {
+#ifdef ROS_DISTRO_GALACTIC
+    using rosidl_generator_traits::to_yaml;
+#endif
+
+    const auto wrapped = [this, callback](typename WrapType::SharedFuture future) {
+      RCLCPP_INFO_STREAM(logger_, "client exit: " << SpecT::name << "\n" << to_yaml(*future.get()));
+      callback(future);
+    };
+
+    RCLCPP_INFO_STREAM(logger_, "client call: " << SpecT::name << "\n" << to_yaml(*request));
+    return client_->async_send_request(request, wrapped);
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Client)
+  typename WrapType::SharedPtr client_;
+  rclcpp::Logger logger_;
+};
+
+}  // namespace component_interface_utils
+
+#endif  // COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_CLIENT_HPP_

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

@@ -15,7 +15,8 @@
 #ifndef COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_SERVER_HPP_
 #define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_SERVER_HPP_
 
-#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/service.hpp>
 
 namespace component_interface_utils
 {
@@ -26,13 +27,11 @@ class Service
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Service)
-
-  template <class NodeT>
-  using CallbackType = void (NodeT::*)(
-    typename SpecT::Service::Request::SharedPtr, typename SpecT::Service::Response::SharedPtr);
+  using SpecType = SpecT;
+  using WrapType = rclcpp::Service<typename SpecT::Service>;
 
   /// Constructor.
-  explicit Service(typename rclcpp::Service<typename SpecT::Service>::SharedPtr service)
+  explicit Service(typename WrapType::SharedPtr service)
   {
     service_ = service;  // to keep the reference count
   }
@@ -56,7 +55,7 @@ class Service
 
 private:
   RCLCPP_DISABLE_COPY(Service)
-  typename rclcpp::Service<typename SpecT::Service>::SharedPtr service_;
+  typename WrapType::SharedPtr service_;
 };
 
 }  // namespace component_interface_utils

common/component_interface_utils/include/component_interface_utils/rclcpp/topic_publisher.hpp

@@ -0,0 +1,48 @@
+// 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__TOPIC_PUBLISHER_HPP_
+#define COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_PUBLISHER_HPP_
+
+#include <rclcpp/publisher.hpp>
+
+namespace component_interface_utils
+{
+
+/// The wrapper class of rclcpp::Publisher. This is for future use and no functionality now.
+template <class SpecT>
+class Publisher
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Publisher)
+  using SpecType = SpecT;
+  using WrapType = rclcpp::Publisher<typename SpecT::Message>;
+
+  /// Constructor.
+  explicit Publisher(typename WrapType::SharedPtr publisher)
+  {
+    publisher_ = publisher;  // to keep the reference count
+  }
+
+  /// Publish a message.
+  void publish(const typename SpecT::Message & msg) { publisher_->publish(msg); }
+
+private:
+  RCLCPP_DISABLE_COPY(Publisher)
+  typename WrapType::SharedPtr publisher_;
+};
+
+}  // namespace component_interface_utils
+
+#endif  // COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_PUBLISHER_HPP_

common/component_interface_utils/include/component_interface_utils/rclcpp/topic_subscription.hpp

@@ -0,0 +1,45 @@
+// 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__TOPIC_SUBSCRIPTION_HPP_
+#define COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_SUBSCRIPTION_HPP_
+
+#include <rclcpp/subscription.hpp>
+
+namespace component_interface_utils
+{
+
+/// The wrapper class of rclcpp::Subscription. This is for future use and no functionality now.
+template <class SpecT>
+class Subscription
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
+  using SpecType = SpecT;
+  using WrapType = rclcpp::Subscription<typename SpecT::Message>;
+
+  /// Constructor.
+  explicit Subscription(typename WrapType::SharedPtr subscription)
+  {
+    subscription_ = subscription;  // to keep the reference count
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Subscription)
+  typename WrapType::SharedPtr subscription_;
+};
+
+}  // namespace component_interface_utils
+
+#endif  // COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_SUBSCRIPTION_HPP_

common/component_interface_utils/include/component_interface_utils/specs.hpp

@@ -0,0 +1,34 @@
+// 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__SPECS_HPP_
+#define COMPONENT_INTERFACE_UTILS__SPECS_HPP_
+
+#include <rclcpp/qos.hpp>
+
+namespace component_interface_utils
+{
+
+template <class SpecT>
+rclcpp::QoS get_qos()
+{
+  rclcpp::QoS qos(SpecT::depth);
+  qos.reliability(SpecT::reliability);
+  qos.durability(SpecT::durability);
+  return qos;
+}
+
+}  // namespace component_interface_utils
+
+#endif  // COMPONENT_INTERFACE_UTILS__SPECS_HPP_