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 +#include #include #include #include #include +#include #include #include @@ -43,14 +45,14 @@ class NodeAdaptor public: /// Constructor. - explicit NodeAdaptor(rclcpp::Node * node) : node_(node) {} + explicit NodeAdaptor(rclcpp::Node * node) { interface_ = std::make_shared(node); } /// Create a client wrapper for logging. template void init_cli(SharedPtrT & cli, CallbackGroup group = nullptr) const { using SpecT = typename SharedPtrT::element_type::SpecType; - cli = create_client_impl(node_, group); + cli = create_client_impl(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(node_, std::forward(callback), group); + srv = create_service_impl(interface_, std::forward(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(node_); + pub = create_publisher_impl(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(node_, std::forward(callback)); + sub = create_subscription_impl(interface_->node, std::forward(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 #include #include #include @@ -28,25 +29,24 @@ namespace component_interface_utils { /// Create a client wrapper for logging. This is a private implementation. -template +template typename Client::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( - SpecT::name, rmw_qos_profile_services_default, group); - return Client::make_shared(client, node); + return Client::make_shared(interface, group); } /// Create a service wrapper for logging. This is a private implementation. -template +template typename Service::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::make_shared(node, std::forward(callback), group); + return Service::make_shared(interface, std::forward(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 + +#include + +#include +#include + +namespace component_interface_utils +{ + +struct NodeInterface +{ + using SharedPtr = std::shared_ptr; + using ServiceLog = tier4_system_msgs::msg::ServiceLog; + + explicit NodeInterface(rclcpp::Node * node) + { + this->node = node; + this->logger = node->create_publisher("/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::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 +#include #include #include @@ -38,12 +39,11 @@ class Client using ServiceLog = tier4_system_msgs::msg::ServiceLog; /// Constructor. - template - 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("/service_log", 10); - src_ = node->get_namespace() + std::string("/") + node->get_name(); + client_ = interface->node->create_client( + SpecT::name, rmw_qos_profile_services_default, group); } /// Send request. @@ -51,7 +51,7 @@ class Client const typename WrapType::SharedRequest request, std::optional 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>(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::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 +#include #include #include @@ -51,14 +52,14 @@ class Service using ServiceLog = tier4_system_msgs::msg::ServiceLog; /// Constructor. - template - Service(NodeT * node, CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr) + template + Service( + NodeInterface::SharedPtr interface, CallbackT && callback, + rclcpp::CallbackGroup::SharedPtr group) + : interface_(interface) { - service_ = node->template create_service( + service_ = interface_->node->create_service( SpecT::name, wrap(callback), rmw_qos_profile_services_default, group); - - pub_ = node->template create_publisher("/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::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::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