From 90c8bb335d0f3dfd7513228828329373f163f627 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <isamu.takagi@tier4.jp>
Date: Wed, 5 Oct 2022 07:38:22 +0900
Subject: [PATCH 1/4] feat(component_interface_utils): change client log

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../component_interface_utils/CMakeLists.txt  |  2 +-
 .../rclcpp/create_interface.hpp               |  2 +-
 .../rclcpp/service_client.hpp                 | 37 ++++++++++++++-----
 .../rclcpp/service_server.hpp                 |  4 ++
 common/component_interface_utils/package.xml  |  2 +
 5 files changed, 35 insertions(+), 12 deletions(-)

diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt
index 96d32c7e04177..d753b4e92d218 100644
--- a/common/component_interface_utils/CMakeLists.txt
+++ b/common/component_interface_utils/CMakeLists.txt
@@ -3,5 +3,5 @@ project(component_interface_utils)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
-
+ament_export_dependencies(tier4_system_msgs)
 ament_auto_package()
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp
index c16abcbe7ad78..0711967db86cb 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp
@@ -36,7 +36,7 @@ typename Client<SpecT>::SharedPtr create_client_impl(
   // 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());
+  return Client<SpecT>::make_shared(client, node);
 }
 
 /// Create a service wrapper for logging. This is a private implementation.
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp
index 6d0e2c21bc6cf..6613e9016999e 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp
@@ -16,11 +16,12 @@
 #define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_CLIENT_HPP_
 
 #include <component_interface_utils/rclcpp/exceptions.hpp>
-#include <rclcpp/client.hpp>
-#include <rclcpp/logger.hpp>
-#include <rclcpp/logging.hpp>
+#include <rclcpp/node.hpp>
+
+#include <tier4_system_msgs/msg/service_log.hpp>
 
 #include <optional>
+#include <string>
 #include <utility>
 
 namespace component_interface_utils
@@ -34,12 +35,15 @@ class Client
   RCLCPP_SMART_PTR_DEFINITIONS(Client)
   using SpecType = SpecT;
   using WrapType = rclcpp::Client<typename SpecT::Service>;
+  using ServiceLog = tier4_system_msgs::msg::ServiceLog;
 
   /// Constructor.
-  explicit Client(typename WrapType::SharedPtr client, const rclcpp::Logger & logger)
-  : logger_(logger)
+  template <class NodeT>
+  Client(typename WrapType::SharedPtr client, NodeT * node)
   {
     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();
   }
 
   /// Send request.
@@ -47,7 +51,7 @@ class Client
     const typename WrapType::SharedRequest request, std::optional<double> timeout = std::nullopt)
   {
     if (!client_->service_is_ready()) {
-      RCLCPP_INFO_STREAM(logger_, "client unready: " << SpecT::name);
+      log(ServiceLog::ERROR_UNREADY);
       throw ServiceUnready(SpecT::name);
     }
 
@@ -55,7 +59,7 @@ class Client
     if (timeout) {
       const auto duration = std::chrono::duration<double, std::ratio<1>>(timeout.value());
       if (future.wait_for(duration) != std::future_status::ready) {
-        RCLCPP_INFO_STREAM(logger_, "client timeout: " << SpecT::name);
+        log(ServiceLog::ERROR_TIMEOUT);
         throw ServiceTimeout(SpecT::name);
       }
     }
@@ -78,11 +82,11 @@ class Client
 #endif
 
     const auto wrapped = [this, callback](typename WrapType::SharedFuture future) {
-      RCLCPP_INFO_STREAM(logger_, "client exit: " << SpecT::name << "\n" << to_yaml(*future.get()));
+      log(ServiceLog::CLIENT_RESPONSE, to_yaml(*future.get()));
       callback(future);
     };
 
-    RCLCPP_INFO_STREAM(logger_, "client call: " << SpecT::name << "\n" << to_yaml(*request));
+    log(ServiceLog::CLIENT_REQUEST, to_yaml(*request));
 
 #ifdef ROS_DISTRO_GALACTIC
     return client_->async_send_request(request, wrapped);
@@ -94,7 +98,20 @@ class Client
 private:
   RCLCPP_DISABLE_COPY(Client)
   typename WrapType::SharedPtr client_;
-  rclcpp::Logger logger_;
+  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);
+  }
 };
 
 }  // namespace component_interface_utils
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
index f8ba8ab980fcb..b6fc0fea754ff 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
@@ -17,8 +17,11 @@
 
 #include <component_interface_utils/rclcpp/exceptions.hpp>
 #include <rclcpp/logging.hpp>
+#include <rclcpp/publisher.hpp>
 #include <rclcpp/service.hpp>
 
+#include <tier4_system_msgs/msg/service_log.hpp>
+
 namespace component_interface_utils
 {
 
@@ -81,6 +84,7 @@ class Service
 private:
   RCLCPP_DISABLE_COPY(Service)
   typename WrapType::SharedPtr service_;
+  rclcpp::Publisher<tier4_system_msgs::msg::ServiceLog>::SharedPtr pub_;
 };
 
 }  // namespace component_interface_utils
diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml
index 94e925b7b4b7a..74f572e8b86b2 100644
--- a/common/component_interface_utils/package.xml
+++ b/common/component_interface_utils/package.xml
@@ -18,6 +18,8 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 
+  <build_export_depend>tier4_system_msgs</build_export_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

From afb16418913958f303cff06596dfcf3bdfec82e5 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <isamu.takagi@tier4.jp>
Date: Wed, 5 Oct 2022 09:48:03 +0900
Subject: [PATCH 2/4] feat(component_interface_utils): change server log

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../rclcpp/create_interface.hpp               |  5 +--
 .../rclcpp/service_server.hpp                 | 37 ++++++++++++++-----
 2 files changed, 29 insertions(+), 13 deletions(-)

diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp
index 0711967db86cb..d9bffd659544d 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp
@@ -46,10 +46,7 @@ typename Service<SpecT>::SharedPtr create_service_impl(
 {
   // 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());
-  auto service = node->template create_service<typename SpecT::Service>(
-    SpecT::name, wrapped, rmw_qos_profile_services_default, group);
-  return Service<SpecT>::make_shared(service);
+  return Service<SpecT>::make_shared(node, std::forward<CallbackT>(callback), group);
 }
 
 /// Create a publisher using traits like services. This is a private implementation.
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
index b6fc0fea754ff..cbc85b8b19cc6 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
@@ -16,12 +16,12 @@
 #define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_SERVER_HPP_
 
 #include <component_interface_utils/rclcpp/exceptions.hpp>
-#include <rclcpp/logging.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/service.hpp>
+#include <rclcpp/node.hpp>
 
 #include <tier4_system_msgs/msg/service_log.hpp>
 
+#include <string>
+
 namespace component_interface_utils
 {
 
@@ -48,25 +48,31 @@ class Service
   RCLCPP_SMART_PTR_DEFINITIONS(Service)
   using SpecType = SpecT;
   using WrapType = rclcpp::Service<typename SpecT::Service>;
+  using ServiceLog = tier4_system_msgs::msg::ServiceLog;
 
   /// Constructor.
-  explicit Service(typename WrapType::SharedPtr service)
+  template <class NodeT, class CallbackT>
+  Service(NodeT * node, CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr)
   {
-    service_ = service;  // to keep the reference count
+    service_ = node->template 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.
   template <class CallbackT>
-  static auto wrap(CallbackT && callback, const rclcpp::Logger & logger)
+  typename WrapType::CallbackType wrap(CallbackT && callback)
   {
-    auto wrapped = [logger, callback](
+    auto wrapped = [this, callback](
                      typename SpecT::Service::Request::SharedPtr request,
                      typename SpecT::Service::Response::SharedPtr response) {
 #ifdef ROS_DISTRO_GALACTIC
       using rosidl_generator_traits::to_yaml;
 #endif
       // If the response has status, convert it from the exception.
-      RCLCPP_INFO_STREAM(logger, "service call: " << SpecT::name << "\n" << to_yaml(*request));
+      log(ServiceLog::SERVER_REQUEST, to_yaml(*request));
       if constexpr (!has_status_type<typename SpecT::Service::Response>::value) {
         callback(request, response);
       } else {
@@ -76,7 +82,7 @@ class Service
           error.set(response->status);
         }
       }
-      RCLCPP_INFO_STREAM(logger, "service exit: " << SpecT::name << "\n" << to_yaml(*response));
+      log(ServiceLog::SERVER_RESPONSE, to_yaml(*response));
     };
     return wrapped;
   }
@@ -85,6 +91,19 @@ class Service
   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);
+  }
 };
 
 }  // namespace component_interface_utils

From 702492927751aeba778ab8f462e3ecb2db4471d9 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <isamu.takagi@tier4.jp>
Date: Wed, 5 Oct 2022 11:40:14 +0900
Subject: [PATCH 3/4] feat(component_interface_utils): add interface

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../component_interface_utils/rclcpp.hpp      | 14 +++--
 .../rclcpp/create_interface.hpp               | 16 ++---
 .../rclcpp/interface.hpp                      | 63 +++++++++++++++++++
 .../rclcpp/service_client.hpp                 | 33 +++-------
 .../rclcpp/service_server.hpp                 | 32 +++-------
 5 files changed, 99 insertions(+), 59 deletions(-)
 create mode 100644 common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp

diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp
index 283b99b6f34cc..32b6b6ca2565c 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp
+++ b/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,14 +45,14 @@ 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.
@@ -58,7 +60,7 @@ class NodeAdaptor
   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.
@@ -66,7 +68,7 @@ class NodeAdaptor
   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.
@@ -74,7 +76,7 @@ class NodeAdaptor
   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
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp
index d9bffd659544d..d5af211b015bf 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp
+++ b/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.
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp
new file mode 100644
index 0000000000000..5a7421a41385d
--- /dev/null
+++ b/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_
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp
index 6613e9016999e..f24f32f407a43 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp
+++ b/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,12 +39,11 @@ 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.
@@ -51,7 +51,7 @@ class Client
     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);
     }
 
@@ -59,7 +59,7 @@ class Client
     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
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
index cbc85b8b19cc6..762f95fe8b450 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp
+++ b/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,7 +83,7 @@ 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;
   }
@@ -90,20 +91,7 @@ class Service
 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

From e122d5bdc9cc9a97e1d8508f365e23b2f77e01a4 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <isamu.takagi@tier4.jp>
Date: Wed, 12 Oct 2022 16:46:34 +0900
Subject: [PATCH 4/4] feat: add console log

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../rclcpp/interface.hpp                      | 24 +++++++++++++------
 .../rclcpp/service_client.hpp                 |  4 ++--
 2 files changed, 19 insertions(+), 9 deletions(-)

diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp
index 5a7421a41385d..bc48e2a0294e0 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp
@@ -21,6 +21,7 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 
 namespace component_interface_utils
 {
@@ -35,27 +36,36 @@ struct NodeInterface
     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_namespace();
+    if (node_name.empty() || node_name.back() != '/') {
+      node_name += "/";
     }
-    node_name_ += node->get_name();
+    node_name += node->get_name();
   }
 
-  void log(ServiceLog::_type_type type, const std::string & name, const std::string & yaml)
+  void log(ServiceLog::_type_type type, const std::string & name, const std::string & yaml = "")
   {
+    static const auto type_text = std::unordered_map<ServiceLog::_type_type, std::string>(
+      {{ServiceLog::CLIENT_REQUEST, "client call"},
+       {ServiceLog::SERVER_REQUEST, "server call"},
+       {ServiceLog::SERVER_RESPONSE, "server exit"},
+       {ServiceLog::CLIENT_RESPONSE, "client exit"},
+       {ServiceLog::ERROR_UNREADY, "client unready"},
+       {ServiceLog::ERROR_TIMEOUT, "client timeout"}});
+    RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name);
+
     ServiceLog msg;
     msg.stamp = node->now();
     msg.type = type;
     msg.name = name;
-    msg.node = node_name_;
+    msg.node = node_name;
     msg.yaml = yaml;
     logger->publish(msg);
   }
 
   rclcpp::Node * node;
   rclcpp::Publisher<ServiceLog>::SharedPtr logger;
-  std::string node_name_;
+  std::string node_name;
 };
 
 }  // namespace component_interface_utils
diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp
index f24f32f407a43..d3333dfbf808d 100644
--- a/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp
+++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp
@@ -51,7 +51,7 @@ class Client
     const typename WrapType::SharedRequest request, std::optional<double> timeout = std::nullopt)
   {
     if (!client_->service_is_ready()) {
-      interface_->log(ServiceLog::ERROR_UNREADY, SpecType::name, "");
+      interface_->log(ServiceLog::ERROR_UNREADY, SpecType::name);
       throw ServiceUnready(SpecT::name);
     }
 
@@ -59,7 +59,7 @@ class Client
     if (timeout) {
       const auto duration = std::chrono::duration<double, std::ratio<1>>(timeout.value());
       if (future.wait_for(duration) != std::future_status::ready) {
-        interface_->log(ServiceLog::ERROR_TIMEOUT, SpecType::name, "");
+        interface_->log(ServiceLog::ERROR_TIMEOUT, SpecType::name);
         throw ServiceTimeout(SpecT::name);
       }
     }