feat(component_interface_utils): apply message change (autowarefoundation#1921)

* feat(component_interface_utils): apply message change

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

* feat: add error category

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

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

Author: isamu-takagi

common/component_interface_utils/include/component_interface_utils/rclcpp.hpp

@@ -32,6 +32,10 @@ class NodeAdaptor
 private:
   using CallbackGroup = rclcpp::CallbackGroup::SharedPtr;
 
+  template <class SharedPtrT, class InstanceT>
+  using MessageCallback =
+    void (InstanceT::*)(const typename SharedPtrT::element_type::SpecType::Message::ConstSharedPtr);
+
   template <class SharedPtrT, class InstanceT>
   using ServiceCallback = void (InstanceT::*)(
     const typename SharedPtrT::element_type::SpecType::Service::Request::SharedPtr,
@@ -92,14 +96,25 @@ class NodeAdaptor
       srv, [cli, timeout](auto req, auto res) { *res = *cli->call(req, timeout); }, group);
   }
 
+  /// Create a subscription wrapper.
+  template <class SharedPtrT, class InstanceT>
+  void init_sub(
+    SharedPtrT & sub, InstanceT * instance,
+    MessageCallback<SharedPtrT, InstanceT> && callback) const
+  {
+    using std::placeholders::_1;
+    init_sub(sub, std::bind(callback, instance, _1));
+  }
+
   /// Create a service wrapper for logging.
   template <class SharedPtrT, class InstanceT>
   void init_srv(
-    SharedPtrT & srv, InstanceT * instance, ServiceCallback<SharedPtrT, InstanceT> callback,
+    SharedPtrT & srv, InstanceT * instance, ServiceCallback<SharedPtrT, InstanceT> && callback,
     CallbackGroup group = nullptr) const
   {
-    init_srv(
-      srv, [instance, callback](auto req, auto res) { (instance->*callback)(req, res); }, group);
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+    init_srv(srv, std::bind(callback, instance, _1, _2), group);
   }
 
 private:

common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp

@@ -34,6 +34,15 @@ class ServiceException : public std::runtime_error
   {
     code_ = code;
   }
+
+  template <class T>
+  void set(T & status) const
+  {
+    status.success = false;
+    status.code = code_;
+    status.message = what();
+  }
+
   ResponseStatus status() const
   {
     ResponseStatus status;

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

@@ -70,7 +70,7 @@ class Service
         try {
           callback(request, response);
         } catch (const ServiceException & error) {
-          response->status = error.status();
+          error.set(response->status);
         }
       }
       RCLCPP_INFO_STREAM(logger, "service exit: " << SpecT::name << "\n" << to_yaml(*response));

common/component_interface_utils/include/component_interface_utils/status.hpp

@@ -0,0 +1,31 @@
+// 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__STATUS_HPP_
+#define COMPONENT_INTERFACE_UTILS__STATUS_HPP_
+
+namespace component_interface_utils::status
+{
+
+template <class T1, class T2>
+void copy(const T1 & src, T2 & dst)  // NOLINT(build/include_what_you_use): cpplint false positive
+{
+  dst->status.success = src->status.success;
+  dst->status.code = src->status.code;
+  dst->status.message = src->status.message;
+}
+
+}  // namespace component_interface_utils::status
+
+#endif  // COMPONENT_INTERFACE_UTILS__STATUS_HPP_