feat(pose_initializer): add a method type to the `/localization/initi…

…alize` service. (autowarefoundation#7048) * change message type of InitializeLocalization in component_interface_specs Signed-off-by: Yamato Ando <[email protected]> * change message type of InitializeLocalizaion in pose_initializer Signed-off-by: Yamato Ando <[email protected]> * modify readme Signed-off-by: Yamato Ando <[email protected]> * style(pre-commit): autofix * delete redundant line Signed-off-by: Yamato Ando <[email protected]> * fix typo Signed-off-by: Yamato Ando <[email protected]> * Update localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp Co-authored-by: Ryohsuke Mitsudome <[email protected]> * add description in readme Signed-off-by: Yamato Ando <[email protected]> * style(pre-commit): autofix * add bash Signed-off-by: Yamato Ando <[email protected]> --------- Signed-off-by: Yamato Ando <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <[email protected]>
Interplai · Jun 3, 2024 · 30dba21 · 30dba21
1 parent 65503cf
commit 30dba21
Show file tree

Hide file tree

Showing 10 changed files with 241 additions and 28 deletions.
diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/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";
 };
 

diff --git a/common/component_interface_specs/package.xml b/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>

diff --git a/localization/pose_initializer/README.md b/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
 
@@ -46,3 +46,86 @@ This node depends on the map height fitter library.
 This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md).
 
 <img src="../../system/default_ad_api/document/images/localization.drawio.svg" alt="drawing" width="800"/>
+
+## Initialize pose via CLI
+
+### Using the GNSS estimated position
+
+```bash
+ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization
+```
+
+The GNSS estimated position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position.
+
+### Using the input position
+
+```bash
+ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization "
+pose_with_covariance:
+  - header:
+      frame_id: map
+    pose:
+      pose:
+        position:
+          x: 89571.1640625
+          y: 42301.1875
+          z: -3.1565165519714355
+        orientation:
+          x: 0.0
+          y: 0.0
+          z: 0.28072773940524687
+          w: 0.9597874433062874
+      covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467]
+method: 0
+"
+```
+
+The input position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position.
+
+### Direct initial position set
+
+```bash
+ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization "
+pose_with_covariance:
+  - header:
+      frame_id: map
+    pose:
+      pose:
+        position:
+          x: 89571.1640625
+          y: 42301.1875
+          z: -3.1565165519714355
+        orientation:
+          x: 0.0
+          y: 0.0
+          z: 0.28072773940524687
+          w: 0.9597874433062874
+      covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.06853891909122467]
+method: 1
+"
+```
+
+The initial position is set directly by the input position without going through localization algorithm.
+
+### Via ros2 topic pub
+
+```bash
+ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
+header:
+  frame_id: map
+pose:
+  pose:
+    position:
+      x: 89571.1640625
+      y: 42301.1875
+      z: 0.0
+    orientation:
+      x: 0.0
+      y: 0.0
+      z: 0.28072773940524687
+      w: 0.9597874433062874
+"
+```
+
+It behaves the same as "initialpose (from rviz)".
+The position.z and the covariance will be overwritten by [ad_api_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/default_ad_api_helpers/ad_api_adaptors), so there is no need to input them.
diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp
@@ -23,6 +23,7 @@
 #include "yabloc_module.hpp"
 
 #include <memory>
+#include <sstream>
 #include <vector>
 
 PoseInitializer::PoseInitializer() : Node("pose_initializer")
@@ -79,7 +80,7 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer")
       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::change_node_trigger(bool flag, bool need_spin)
   }
 }
 
-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 @@ void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Po
     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 @@ void PoseInitializer::on_initialize(
       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);
   }
 }

diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/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,

diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt
@@ -18,6 +18,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
 )

diff --git a/system/default_ad_api/src/localization.cpp b/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

diff --git a/system/default_ad_api/src/localization.hpp b/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

diff --git a/system/default_ad_api/src/utils/localization_conversion.cpp b/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
diff --git a/system/default_ad_api/src/utils/localization_conversion.hpp b/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_