Initial import of FaRui Campus ADS v3.2

2026-06-05 14:20:30 +08:00
commit 2839d34fdb
6548 changed files with 1335203 additions and 0 deletions
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_api_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+if(BUILD_TESTING)
+  include_directories(include)
+  ament_add_ros_isolated_gtest(${PROJECT_NAME}_test test/test.cpp)
+  ament_target_dependencies(${PROJECT_NAME}_test rclcpp tier4_external_api_msgs)
+endif()
+
+ament_auto_package()
@@ -0,0 +1,4 @@
+# tier4_api_utils
+
+This is an old implementation of a class that logs when calling a service.
+Please use [component_interface_utils](../component_interface_utils/README.md) instead.
@@ -0,0 +1,70 @@
+// Copyright 2021 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 TIER4_API_UTILS__RCLCPP__CLIENT_HPP_
+#define TIER4_API_UTILS__RCLCPP__CLIENT_HPP_
+
+#include "rclcpp/client.hpp"
+#include "tier4_api_utils/types/response.hpp"
+
+#include <chrono>
+#include <utility>
+
+namespace tier4_api_utils
+{
+template <typename ServiceT>
+class Client
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Client)
+
+  using ResponseStatus = tier4_external_api_msgs::msg::ResponseStatus;
+  using AutowareServiceResult = std::pair<ResponseStatus, typename ServiceT::Response::SharedPtr>;
+
+  Client(typename rclcpp::Client<ServiceT>::SharedPtr client, const rclcpp::Logger & logger)
+  : client_(client), logger_(logger)
+  {
+  }
+
+  AutowareServiceResult call(
+    const typename ServiceT::Request::SharedPtr & request,
+    const std::chrono::nanoseconds & timeout = std::chrono::seconds(2))
+  {
+    RCLCPP_DEBUG(logger_, "client request");
+
+    if (!client_->service_is_ready()) {
+      RCLCPP_DEBUG(logger_, "client available");
+      return {response_error("Internal service is not available."), nullptr};
+    }
+
+    auto future = client_->async_send_request(request);
+    if (future.wait_for(timeout) != std::future_status::ready) {
+      RCLCPP_DEBUG(logger_, "client timeout");
+      return {response_error("Internal service has timed out."), nullptr};
+    }
+
+    RCLCPP_DEBUG(logger_, "client response");
+    return {response_success(), future.get()};
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Client)
+
+  typename rclcpp::Client<ServiceT>::SharedPtr client_;
+  rclcpp::Logger logger_;
+};
+
+}  // namespace tier4_api_utils
+
+#endif  // TIER4_API_UTILS__RCLCPP__CLIENT_HPP_
@@ -0,0 +1,63 @@
+// Copyright 2021 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 TIER4_API_UTILS__RCLCPP__PROXY_HPP_
+#define TIER4_API_UTILS__RCLCPP__PROXY_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_api_utils/rclcpp/client.hpp"
+#include "tier4_api_utils/rclcpp/service.hpp"
+
+#include <string>
+#include <utility>
+
+namespace tier4_api_utils
+{
+template <class NodeT>
+class ServiceProxyNodeInterface
+{
+public:
+  // Use a raw pointer because shared_from_this cannot be used in constructor.
+  explicit ServiceProxyNodeInterface(NodeT * node) { node_ = node; }
+
+  template <typename ServiceT, typename CallbackT>
+  typename Service<ServiceT>::SharedPtr create_service(
+    const std::string & service_name, CallbackT && callback,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  {
+    auto wrapped_callback = Service<ServiceT>::template wrap<CallbackT>(
+      std::forward<CallbackT>(callback), node_->get_logger());
+    return Service<ServiceT>::make_shared(node_->template create_service<ServiceT>(
+      service_name, std::move(wrapped_callback), qos_profile, group));
+  }
+
+  template <typename ServiceT>
+  typename Client<ServiceT>::SharedPtr create_client(
+    const std::string & service_name,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  {
+    return Client<ServiceT>::make_shared(
+      node_->template create_client<ServiceT>(service_name, qos_profile, group),
+      node_->get_logger());
+  }
+
+private:
+  NodeT * node_;
+};
+
+}  // namespace tier4_api_utils
+
+#endif  // TIER4_API_UTILS__RCLCPP__PROXY_HPP_
@@ -0,0 +1,51 @@
+// Copyright 2021 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 TIER4_API_UTILS__RCLCPP__SERVICE_HPP_
+#define TIER4_API_UTILS__RCLCPP__SERVICE_HPP_
+
+#include "rclcpp/service.hpp"
+
+namespace tier4_api_utils
+{
+template <typename ServiceT>
+class Service
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Service)
+
+  explicit Service(typename rclcpp::Service<ServiceT>::SharedPtr service) : service_(service) {}
+
+  template <typename CallbackT>
+  static auto wrap(CallbackT && callback, const rclcpp::Logger & logger)
+  {
+    auto wrapped_callback = [logger, callback](
+                              typename ServiceT::Request::SharedPtr request,
+                              typename ServiceT::Response::SharedPtr response) {
+      RCLCPP_INFO(logger, "service request");
+      callback(request, response);
+      RCLCPP_INFO(logger, "service response");
+    };
+    return wrapped_callback;
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Service)
+
+  typename rclcpp::Service<ServiceT>::SharedPtr service_;
+};
+
+}  // namespace tier4_api_utils
+
+#endif  // TIER4_API_UTILS__RCLCPP__SERVICE_HPP_
@@ -0,0 +1,21 @@
+// Copyright 2021 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 TIER4_API_UTILS__TIER4_API_UTILS_HPP_
+#define TIER4_API_UTILS__TIER4_API_UTILS_HPP_
+
+#include "tier4_api_utils/rclcpp/proxy.hpp"
+#include "tier4_api_utils/types/response.hpp"
+
+#endif  // TIER4_API_UTILS__TIER4_API_UTILS_HPP_
@@ -0,0 +1,78 @@
+// Copyright 2021 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 TIER4_API_UTILS__TYPES__RESPONSE_HPP_
+#define TIER4_API_UTILS__TYPES__RESPONSE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "tier4_external_api_msgs/msg/response_status.hpp"
+
+#include <string>
+
+namespace tier4_api_utils
+{
+using ResponseStatus = tier4_external_api_msgs::msg::ResponseStatus;
+
+inline bool is_success(const tier4_external_api_msgs::msg::ResponseStatus & status)
+{
+  return status.code == tier4_external_api_msgs::msg::ResponseStatus::SUCCESS;
+}
+
+inline bool is_ignored(const tier4_external_api_msgs::msg::ResponseStatus & status)
+{
+  return status.code == tier4_external_api_msgs::msg::ResponseStatus::IGNORED;
+}
+
+inline bool is_warn(const tier4_external_api_msgs::msg::ResponseStatus & status)
+{
+  return status.code == tier4_external_api_msgs::msg::ResponseStatus::WARN;
+}
+
+inline bool is_error(const tier4_external_api_msgs::msg::ResponseStatus & status)
+{
+  return status.code == tier4_external_api_msgs::msg::ResponseStatus::ERROR;
+}
+
+inline ResponseStatus response_success(const std::string & message = "")
+{
+  return tier4_external_api_msgs::build<tier4_external_api_msgs::msg::ResponseStatus>()
+    .code(tier4_external_api_msgs::msg::ResponseStatus::SUCCESS)
+    .message(message);
+}
+
+inline ResponseStatus response_ignored(const std::string & message = "")
+{
+  return tier4_external_api_msgs::build<tier4_external_api_msgs::msg::ResponseStatus>()
+    .code(tier4_external_api_msgs::msg::ResponseStatus::IGNORED)
+    .message(message);
+}
+
+inline ResponseStatus response_warn(const std::string & message = "")
+{
+  return tier4_external_api_msgs::build<tier4_external_api_msgs::msg::ResponseStatus>()
+    .code(tier4_external_api_msgs::msg::ResponseStatus::WARN)
+    .message(message);
+}
+
+inline ResponseStatus response_error(const std::string & message = "")
+{
+  return tier4_external_api_msgs::build<tier4_external_api_msgs::msg::ResponseStatus>()
+    .code(tier4_external_api_msgs::msg::ResponseStatus::ERROR)
+    .message(message);
+}
+
+}  // namespace tier4_api_utils
+
+#endif  // TIER4_API_UTILS__TYPES__RESPONSE_HPP_
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>tier4_api_utils</name>
+  <version>0.0.0</version>
+  <description>The tier4_api_utils package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>tier4_external_api_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>rclcpp</test_depend>
+  <test_depend>tier4_external_api_msgs</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,32 @@
+// Copyright 2021 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.
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_api_utils/tier4_api_utils.hpp"
+
+TEST(tier4_api_utils, instantiate)
+{
+  rclcpp::Node node("tier4_api_utils_test");
+  tier4_api_utils::ServiceProxyNodeInterface proxy(&node);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  bool result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}