feat(component_interface_utils): change service log output (#2022)

* feat(component_interface_utils): change client log Signed-off-by: Takagi, Isamu <[email protected]> * feat(component_interface_utils): change server log Signed-off-by: Takagi, Isamu <[email protected]> * feat(component_interface_utils): add interface Signed-off-by: Takagi, Isamu <[email protected]> * feat: add console log Signed-off-by: Takagi, Isamu <[email protected]> Signed-off-by: Takagi, Isamu <[email protected]>
autowarefoundation · Oct 13, 2022 · 0f16c2b · 0f16c2b
1 parent 4be6931
commit 0f16c2b
Show file tree

Hide file tree

Showing 7 changed files with 126 additions and 37 deletions.
diff --git 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.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,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

diff --git a/...n/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp b/...n/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,28 +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->get_logger());
+  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
-  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(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
@@ -0,0 +1,73 @@
+// 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>
+#include <unordered_map>
+
+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 = "")
+  {
+    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.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
@@ -16,11 +16,13 @@
 #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 <component_interface_utils/rclcpp/interface.hpp>
+#include <rclcpp/node.hpp>
+
+#include <tier4_system_msgs/msg/service_log.hpp>
 
 #include <optional>
+#include <string>
 #include <utility>
 
 namespace component_interface_utils
@@ -34,28 +36,30 @@ 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)
+  Client(NodeInterface::SharedPtr interface, rclcpp::CallbackGroup::SharedPtr group)
+  : interface_(interface)
   {
-    client_ = client;  // to keep the reference count
+    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()) {
-      RCLCPP_INFO_STREAM(logger_, "client unready: " << SpecT::name);
+      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) {
-        RCLCPP_INFO_STREAM(logger_, "client timeout: " << SpecT::name);
+        interface_->log(ServiceLog::ERROR_TIMEOUT, SpecType::name);
         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()));
+      interface_->log(ServiceLog::CLIENT_RESPONSE, SpecType::name, to_yaml(*future.get()));
       callback(future);
     };
 
-    RCLCPP_INFO_STREAM(logger_, "client call: " << SpecT::name << "\n" << to_yaml(*request));
+    interface_->log(ServiceLog::CLIENT_REQUEST, SpecType::name, to_yaml(*request));
 
 #ifdef ROS_DISTRO_GALACTIC
     return client_->async_send_request(request, wrapped);
@@ -94,7 +98,7 @@ class Client
 private:
   RCLCPP_DISABLE_COPY(Client)
   typename WrapType::SharedPtr client_;
-  rclcpp::Logger logger_;
+  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
@@ -16,8 +16,12 @@
 #define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_SERVER_HPP_
 
 #include <component_interface_utils/rclcpp/exceptions.hpp>
-#include <rclcpp/logging.hpp>
-#include <rclcpp/service.hpp>
+#include <component_interface_utils/rclcpp/interface.hpp>
+#include <rclcpp/node.hpp>
+
+#include <tier4_system_msgs/msg/service_log.hpp>
+
+#include <string>
 
 namespace component_interface_utils
 {
@@ -45,25 +49,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 CallbackT>
+  Service(
+    NodeInterface::SharedPtr interface, CallbackT && callback,
+    rclcpp::CallbackGroup::SharedPtr group)
+  : interface_(interface)
   {
-    service_ = service;  // to keep the reference count
+    service_ = interface_->node->create_service<typename SpecT::Service>(
+      SpecT::name, wrap(callback), rmw_qos_profile_services_default, group);
   }
 
   /// 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));
+      interface_->log(ServiceLog::SERVER_REQUEST, SpecType::name, to_yaml(*request));
       if constexpr (!has_status_type<typename SpecT::Service::Response>::value) {
         callback(request, response);
       } else {
@@ -73,14 +83,15 @@ class Service
           error.set(response->status);
         }
       }
-      RCLCPP_INFO_STREAM(logger, "service exit: " << SpecT::name << "\n" << to_yaml(*response));
+      interface_->log(ServiceLog::SERVER_RESPONSE, SpecType::name, to_yaml(*response));
     };
     return wrapped;
   }
 
 private:
   RCLCPP_DISABLE_COPY(Service)
   typename WrapType::SharedPtr service_;
+  NodeInterface::SharedPtr interface_;
 };
 
 }  // namespace component_interface_utils

diff --git 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>