Merge pull request #88 from tier4/sync-upstream

chore: sync upstream

build_depends.humble.repos

@@ -8,6 +8,10 @@ repositories:
     type: git
     url: https://github.com/autowarefoundation/autoware.core.git
     version: main
+  core/autoware_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_msgs.git
+    version: main
   core/external/autoware_auto_msgs:
     type: git
     url: https://github.com/tier4/autoware_auto_msgs.git
@@ -29,3 +33,7 @@ repositories:
     type: git
     url: https://github.com/tier4/ndt_omp.git
     version: tier4/main
+  universe/external/morai_msgs:
+    type: git
+    url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
+    version: main

build_depends.repos

@@ -8,6 +8,10 @@ repositories:
     type: git
     url: https://github.com/autowarefoundation/autoware.core.git
     version: main
+  core/autoware_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_msgs.git
+    version: main
   core/external/autoware_auto_msgs:
     type: git
     url: https://github.com/tier4/autoware_auto_msgs.git
@@ -29,3 +33,7 @@ repositories:
     type: git
     url: https://github.com/tier4/ndt_omp.git
     version: tier4/main
+  universe/external/morai_msgs:
+    type: git
+    url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
+    version: main

common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg

@@ -1,5 +1,8 @@
 # constants for code
 uint16 DEPRECATED = 50000
+uint16 SERVICE_UNREADY = 50001
+uint16 SERVICE_TIMEOUT = 50002
+uint16 TRANSFORM_ERROR = 50003
 
 # variables
 bool   success

common/component_interface_utils/README.md

@@ -4,29 +4,98 @@
 
 This is a utility package that provides the following features:
 
+- Instantiation of the wrapper class
 - Logging for service and client
+- Service exception for response
+- Relays for topic and service
 
-## Usage
+## Design
 
-1. This package requires interface information in this format.
+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.
 
-   ```cpp
-   struct SampleService
-   {
-     using Service = sample_msgs::srv::ServiceType;
-     static constexpr char name[] = "/sample/service";
-   };
-   ```
+## Instantiation of the wrapper class
 
-2. Create a wrapper using the above definition as follows.
+The wrapper class requires interface information in this format.
 
-   ```cpp
-   component_interface_utils::Service<SampleService>::SharedPtr srv_;
-   srv_ = component_interface_utils::create_service<SampleService>(node, ...);
-   ```
+```cpp
+struct SampleService
+{
+  using Service = sample_msgs::srv::ServiceType;
+  static constexpr char name[] = "/sample/service";
+};
 
-## Design
+struct SampleMessage
+{
+  using Message = sample_msgs::msg::MessageType;
+  static constexpr char name[] = "/sample/message";
+  static constexpr size_t depth = 3;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+```
 
-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.
+Create the wrapper using the above definition as follows.
+
+```cpp
+// header file
+component_interface_utils::Service<SampleService>::SharedPtr srv_;
+component_interface_utils::Client<SampleService>::SharedPtr cli_;
+component_interface_utils::Publisher<SampleMessage>::SharedPtr pub_;
+component_interface_utils::Subscription<SampleMessage>::SharedPtr sub_;
+
+// source file
+const auto node = component_interface_utils::NodeAdaptor(this);
+node.init_srv(srv_, callback);
+node.init_cli(cli_);
+node.init_pub(pub_);
+node.init_sub(sub_, callback);
+```
+
+## Logging for service and client
+
+If the wrapper class is used, logging is automatically enabled. The log level is `RCLCPP_INFO`.
+
+## Service exception for response
+
+If the wrapper class is used and the service response has status, throwing `ServiceException` will automatically catch and set it to status.
+This is useful when returning an error from a function called from the service callback.
+
+```cpp
+void service_callback(Request req, Response res)
+{
+   function();
+   res->status.success = true;
+}
+
+void function()
+{
+   throw ServiceException(ERROR_CODE, "message");
+}
+```
+
+If the wrapper class is not used or the service response has no status, manually catch the `ServiceException` as follows.
+
+```cpp
+void service_callback(Request req, Response res)
+{
+   try {
+      function();
+      res->status.success = true;
+   } catch (const ServiceException & error) {
+      res->status = error.status();
+   }
+}
+```
+
+## Relays for topic and service
+
+There are utilities for relaying services and messages of the same type.
+
+```cpp
+const auto node = component_interface_utils::NodeAdaptor(this);
+service_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+node.relay_message(pub_, sub_);
+node.relay_service(cli_, srv_, service_callback_group_);  // group is for avoiding deadlocks
+```

common/component_interface_utils/include/component_interface_utils/rclcpp.hpp

@@ -21,6 +21,7 @@
 #include <component_interface_utils/rclcpp/topic_publisher.hpp>
 #include <component_interface_utils/rclcpp/topic_subscription.hpp>
 
+#include <optional>
 #include <utility>
 
 namespace component_interface_utils
@@ -31,6 +32,11 @@ class NodeAdaptor
 private:
   using CallbackGroup = rclcpp::CallbackGroup::SharedPtr;
 
+  template <class SharedPtrT, class InstanceT>
+  using ServiceCallback = void (InstanceT::*)(
+    const typename SharedPtrT::element_type::SpecType::Service::Request::SharedPtr,
+    const typename SharedPtrT::element_type::SpecType::Service::Response::SharedPtr);
+
 public:
   /// Constructor.
   explicit NodeAdaptor(rclcpp::Node * node) : node_(node) {}
@@ -67,6 +73,37 @@ class NodeAdaptor
     sub = create_subscription_impl<SpecT>(node_, std::forward<CallbackT>(callback));
   }
 
+  /// Relay message.
+  template <class P, class S>
+  void relay_message(P & pub, S & sub) const
+  {
+    using MsgT = typename P::element_type::SpecType::Message::ConstSharedPtr;
+    init_pub(pub);
+    init_sub(sub, [pub](MsgT msg) { pub->publish(*msg); });
+  }
+
+  /// Relay service.
+  template <class C, class S>
+  void relay_service(
+    C & cli, S & srv, CallbackGroup group, std::optional<double> timeout = std::nullopt) const
+  {
+    using ReqT = typename C::element_type::SpecType::Service::Request::SharedPtr;
+    using ResT = typename C::element_type::SpecType::Service::Response::SharedPtr;
+    init_cli(cli);
+    init_srv(
+      srv, [cli, timeout](ReqT req, ResT res) { *res = *cli->call(req, timeout); }, group);
+  }
+
+  /// Create a service wrapper for logging.
+  template <class SharedPtrT, class InstanceT>
+  void init_srv(
+    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);
+  }
+
 private:
   // Use a node pointer because shared_from_this cannot be used in constructor.
   rclcpp::Node * node_;

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

@@ -0,0 +1,79 @@
+// 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__EXCEPTIONS_HPP_
+#define COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_
+
+#include <autoware_ad_api_msgs/msg/response_status.hpp>
+
+#include <stdexcept>
+#include <string>
+
+namespace component_interface_utils
+{
+
+class ServiceException : public std::runtime_error
+{
+public:
+  using ResponseStatus = autoware_ad_api_msgs::msg::ResponseStatus;
+  using ResponseStatusCode = ResponseStatus::_code_type;
+
+  ServiceException(ResponseStatusCode code, const std::string & message)
+  : std::runtime_error(message)
+  {
+    code_ = code;
+  }
+  ResponseStatus status() const
+  {
+    ResponseStatus status;
+    status.success = false;
+    status.code = code_;
+    status.message = what();
+    return status;
+  }
+
+private:
+  ResponseStatusCode code_;
+};
+
+class ServiceUnready : public ServiceException
+{
+public:
+  explicit ServiceUnready(const std::string & message)
+  : ServiceException(ResponseStatus::SERVICE_UNREADY, message)
+  {
+  }
+};
+
+class ServiceTimeout : public ServiceException
+{
+public:
+  explicit ServiceTimeout(const std::string & message)
+  : ServiceException(ResponseStatus::SERVICE_TIMEOUT, message)
+  {
+  }
+};
+
+class TransformError : public ServiceException
+{
+public:
+  explicit TransformError(const std::string & message)
+  : ServiceException(ResponseStatus::TRANSFORM_ERROR, message)
+  {
+  }
+};
+
+}  // namespace component_interface_utils
+
+#endif  // COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_

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

@@ -15,10 +15,12 @@
 #ifndef COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_CLIENT_HPP_
 #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 <optional>
 #include <utility>
 
 namespace component_interface_utils
@@ -40,11 +42,30 @@ class Client
     client_ = client;  // to keep the reference count
   }
 
+  /// 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);
+      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);
+        throw ServiceTimeout(SpecT::name);
+      }
+    }
+    return future.get();
+  }
+
   /// 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);
+    return this->async_send_request(request, [](typename WrapType::SharedFuture) {});
   }
 
   /// Send request.
@@ -62,7 +83,12 @@ class Client
     };
 
     RCLCPP_INFO_STREAM(logger_, "client call: " << SpecT::name << "\n" << to_yaml(*request));
+
+#ifdef ROS_DISTRO_GALACTIC
     return client_->async_send_request(request, wrapped);
+#else
+    return client_->async_send_request(request, wrapped).future;
+#endif
   }
 
 private: