feat(pose_initializer): add a method type to the /localization/initialize service.

common/component_interface_specs/include/component_interface_specs/localization.hpp

@@ -18,16 +18,16 @@
 #include <rclcpp/qos.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
-#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_localization_msgs/srv/initialize_localization.hpp>
 
 namespace localization_interface
 {
 
 struct Initialize
 {
-  using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization;
+  using Service = tier4_localization_msgs::srv::InitializeLocalization;
   static constexpr char name[] = "/localization/initialize";
 };
 

common/component_interface_specs/package.xml

@@ -21,6 +21,7 @@
   <depend>rclcpp</depend>
   <depend>rosidl_runtime_cpp</depend>
   <depend>tier4_control_msgs</depend>
+  <depend>tier4_localization_msgs</depend>
   <depend>tier4_map_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>tier4_system_msgs</depend>

localization/pose_initializer/README.md

@@ -17,9 +17,9 @@ This node depends on the map height fitter library.
 
 ### Services
 
-| Name                       | Type                                                | Description           |
-| -------------------------- | --------------------------------------------------- | --------------------- |
-| `/localization/initialize` | autoware_adapi_v1_msgs::srv::InitializeLocalization | initial pose from api |
+| Name                       | Type                                                 | Description           |
+| -------------------------- | ---------------------------------------------------- | --------------------- |
+| `/localization/initialize` | tier4_localization_msgs::srv::InitializeLocalization | initial pose from api |
 
 ### Clients
 

localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 The Autoware Contributors
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -23,6 +23,7 @@
 #include "yabloc_module.hpp"
 
 #include <memory>
+#include <sstream>
 #include <vector>
 
 PoseInitializer::PoseInitializer() : Node("pose_initializer")
@@ -79,7 +80,7 @@
       initial_pose.orientation.z = initial_pose_array[5];
       initial_pose.orientation.w = initial_pose_array[6];
 
-      set_user_defined_initial_pose(initial_pose);
+      set_user_defined_initial_pose(initial_pose, true);
     }
   }
 }
@@ -114,11 +115,12 @@
   }
 }
 
-void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose)
+void PoseInitializer::set_user_defined_initial_pose(
+  const geometry_msgs::msg::Pose initial_pose, bool need_spin)
 {
   try {
     change_state(State::Message::INITIALIZING);
-    change_node_trigger(false, true);
+    change_node_trigger(false, need_spin);
 
     PoseWithCovarianceStamped pose;
     pose.header.frame_id = "map";
@@ -127,7 +129,7 @@
     pose.pose.covariance = output_pose_covariance_;
     pub_reset_->publish(pose);
 
-    change_node_trigger(true, true);
+    change_node_trigger(true, need_spin);
     change_state(State::Message::INITIALIZED);
 
     RCLCPP_INFO(get_logger(), "Set user defined initial pose");
@@ -147,25 +149,52 @@
       Initialize::Service::Response::ERROR_UNSAFE, "The vehicle is not stopped.");
   }
   try {
-    change_state(State::Message::INITIALIZING);
-    change_node_trigger(false, false);
-
-    auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front();
-    if (ndt_) {
-      pose = ndt_->align_pose(pose);
-    } else if (yabloc_) {
-      // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more
-      // accuracy pose.
-      pose = yabloc_->align_pose(pose);
-    }
-    pose.pose.covariance = output_pose_covariance_;
-    pub_reset_->publish(pose);
+    if (req->method == Initialize::Service::Request::AUTO) {
+      change_state(State::Message::INITIALIZING);
+      change_node_trigger(false, false);
+
+      auto pose =
+        req->pose_with_covariance.empty() ? get_gnss_pose() : req->pose_with_covariance.front();
+      if (ndt_) {
+        pose = ndt_->align_pose(pose);
+      } else if (yabloc_) {
+        // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more
+        // accuracy pose.
+        pose = yabloc_->align_pose(pose);
+      }
+      pose.pose.covariance = output_pose_covariance_;
+      pub_reset_->publish(pose);
+
+      change_node_trigger(true, false);
+      res->status.success = true;
+      change_state(State::Message::INITIALIZED);
+
+    } else if (req->method == Initialize::Service::Request::DIRECT) {
+      if (req->pose_with_covariance.empty()) {
+        std::stringstream message;
+        message << "No input pose_with_covariance. If you want to use DIRECT method, please input "
+                   "pose_with_covariance.";
+        RCLCPP_ERROR(get_logger(), message.str().c_str());
+        throw ServiceException(
+          autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str());
+      }
+      auto pose = req->pose_with_covariance.front().pose.pose;
+      set_user_defined_initial_pose(pose, false);
+      res->status.success = true;
 
-    change_node_trigger(true, false);
-    res->status.success = true;
-    change_state(State::Message::INITIALIZED);
+    } else {
+      std::stringstream message;
+      message << "Unknown method type (=" << std::to_string(req->method) << ")";
+      RCLCPP_ERROR(get_logger(), message.str().c_str());
+      throw ServiceException(
+        autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str());
+    }
   } catch (const ServiceException & error) {
-    res->status = error.status();
+    autoware_adapi_v1_msgs::msg::ResponseStatus respose_status;
+    respose_status = error.status();
+    res->status.success = respose_status.success;
+    res->status.code = respose_status.code;
+    res->status.message = respose_status.message;
     change_state(State::Message::UNINITIALIZED);
   }
 }

localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp

@@ -60,7 +60,8 @@ class PoseInitializer : public rclcpp::Node
   double stop_check_duration_;
 
   void change_node_trigger(bool flag, bool need_spin = false);
-  void set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose);
+  void set_user_defined_initial_pose(
+    const geometry_msgs::msg::Pose initial_pose, bool need_spin = false);
   void change_state(State::Message::_state_type state);
   void on_initialize(
     const Initialize::Service::Request::SharedPtr req,

system/default_ad_api/CMakeLists.txt

@@ -16,6 +16,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/vehicle.cpp
   src/vehicle_info.cpp
   src/vehicle_door.cpp
+  src/utils/localization_conversion.cpp
   src/utils/route_conversion.cpp
   src/compatibility/autoware_state.cpp
 )

system/default_ad_api/src/localization.cpp

@@ -14,6 +14,8 @@
 
 #include "localization.hpp"
 
+#include "utils/localization_conversion.hpp"
+
 namespace default_ad_api
 {
 
@@ -23,7 +25,15 @@ LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options)
   const auto adaptor = component_interface_utils::NodeAdaptor(this);
   group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   adaptor.relay_message(pub_state_, sub_state_);
-  adaptor.relay_service(cli_initialize_, srv_initialize_, group_cli_);
+  adaptor.init_cli(cli_initialize_);
+  adaptor.init_srv(srv_initialize_, this, &LocalizationNode::on_initialize, group_cli_);
+}
+
+void LocalizationNode::on_initialize(
+  const autoware_ad_api::localization::Initialize::Service::Request::SharedPtr req,
+  const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res)
+{
+  res->status = localization_conversion::convert_call(cli_initialize_, req);
 }
 
 }  // namespace default_ad_api

system/default_ad_api/src/localization.hpp

@@ -36,6 +36,10 @@ class LocalizationNode : public rclcpp::Node
   Pub<autoware_ad_api::localization::InitializationState> pub_state_;
   Cli<localization_interface::Initialize> cli_initialize_;
   Sub<localization_interface::InitializationState> sub_state_;
+
+  void on_initialize(
+    const autoware_ad_api::localization::Initialize::Service::Request::SharedPtr req,
+    const autoware_ad_api::localization::Initialize::Service::Response::SharedPtr res);
 };
 
 }  // namespace default_ad_api

system/default_ad_api/src/utils/localization_conversion.cpp

@@ -0,0 +1,40 @@
+// Copyright 2024 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 "localization_conversion.hpp"
+
+#include <memory>
+
+namespace default_ad_api::localization_conversion
+{
+
+InternalInitializeRequest convert_request(const ExternalInitializeRequest & external)
+{
+  auto internal = std::make_shared<InternalInitializeRequest::element_type>();
+  internal->pose_with_covariance = external->pose;
+  internal->method = tier4_localization_msgs::srv::InitializeLocalization::Request::AUTO;
+  return internal;
+}
+
+ExternalResponse convert_response(const InternalResponse & internal)
+{
+  // TODO(Takagi, Isamu): check error code correspondence
+  ExternalResponse external;
+  external.success = internal.success;
+  external.code = internal.code;
+  external.message = internal.message;
+  return external;
+}
+
+}  // namespace default_ad_api::localization_conversion

system/default_ad_api/src/utils/localization_conversion.hpp

@@ -0,0 +1,44 @@
+// Copyright 2024 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 UTILS__LOCALIZATION_CONVERSION_HPP_
+#define UTILS__LOCALIZATION_CONVERSION_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
+#include <tier4_localization_msgs/srv/initialize_localization.hpp>
+
+namespace default_ad_api::localization_conversion
+{
+
+using ExternalInitializeRequest =
+  autoware_adapi_v1_msgs::srv::InitializeLocalization::Request::SharedPtr;
+using InternalInitializeRequest =
+  tier4_localization_msgs::srv::InitializeLocalization::Request::SharedPtr;
+InternalInitializeRequest convert_request(const ExternalInitializeRequest & external);
+
+using ExternalResponse = autoware_adapi_v1_msgs::msg::ResponseStatus;
+using InternalResponse = autoware_common_msgs::msg::ResponseStatus;
+ExternalResponse convert_response(const InternalResponse & internal);
+
+template <class ClientT, class RequestT>
+ExternalResponse convert_call(ClientT & client, RequestT & req)
+{
+  return convert_response(client->call(convert_request(req))->status);
+}
+
+}  // namespace default_ad_api::localization_conversion
+
+#endif  // UTILS__LOCALIZATION_CONVERSION_HPP_