From abf3893a955f1a90e9f06abd4306536ac833e2cc Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 17 May 2024 16:18:58 +0900 Subject: [PATCH 01/10] change message type of InitializeLocalization in component_interface_specs Signed-off-by: Yamato Ando --- .../localization.hpp | 4 +- common/component_interface_specs/package.xml | 1 + system/default_ad_api/CMakeLists.txt | 1 + system/default_ad_api/src/localization.cpp | 12 +++++- system/default_ad_api/src/localization.hpp | 6 +++ .../src/utils/localization_conversion.cpp | 40 ++++++++++++++++++ .../src/utils/localization_conversion.hpp | 42 +++++++++++++++++++ 7 files changed, 103 insertions(+), 3 deletions(-) create mode 100644 system/default_ad_api/src/utils/localization_conversion.cpp create mode 100644 system/default_ad_api/src/utils/localization_conversion.hpp diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp index 70c590c837927..0cdd5f7054ca8 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -27,7 +27,7 @@ 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 index 1ad5f410a814a..f9d690d8c9ebf 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -22,6 +22,7 @@ rosidl_runtime_cpp tier4_control_msgs tier4_map_msgs + tier4_localization_msgs tier4_planning_msgs tier4_system_msgs tier4_vehicle_msgs diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 982112189412e..49a4cc81aaec2 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/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 ) diff --git a/system/default_ad_api/src/localization.cpp b/system/default_ad_api/src/localization.cpp index 468e02b37644f..97544a7868d9c 100644 --- 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 index 71517c4c6c769..52f804cc1e0b9 100644 --- a/system/default_ad_api/src/localization.hpp +++ b/system/default_ad_api/src/localization.hpp @@ -36,6 +36,12 @@ class LocalizationNode : public rclcpp::Node Pub pub_state_; Cli cli_initialize_; Sub 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 new file mode 100644 index 0000000000000..3ddf259a4a590 --- /dev/null +++ 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 + +namespace default_ad_api::localization_conversion +{ + +InternalInitializeRequest convert_request(const ExternalInitializeRequest & external) +{ + auto internal = std::make_shared(); + 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 new file mode 100644 index 0000000000000..baa3d34676de5 --- /dev/null +++ b/system/default_ad_api/src/utils/localization_conversion.hpp @@ -0,0 +1,42 @@ +// 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 + +#include +#include + +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 +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_ From 8400a8a7d6ce0166cd8e98cbbdb176eb895ccb23 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 17 May 2024 16:22:48 +0900 Subject: [PATCH 02/10] change message type of InitializeLocalizaion in pose_initializer Signed-off-by: Yamato Ando --- .../pose_initializer_core.cpp | 65 +++++++++++++------ .../pose_initializer_core.hpp | 2 +- 2 files changed, 46 insertions(+), 21 deletions(-) diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index bc86b5476dcca..452c373d5898f 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -22,6 +22,7 @@ #include "stop_check_module.hpp" #include "yabloc_module.hpp" +#include #include #include @@ -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,11 @@ 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 +128,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 +148,49 @@ 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); + 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 imput pose_with_covariance. If you wanna 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; + + } 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()); } - pose.pose.covariance = output_pose_covariance_; - pub_reset_->publish(pose); - change_node_trigger(true, false); - res->status.success = true; - change_state(State::Message::INITIALIZED); } catch (const ServiceException & error) { - res->status = error.status(); + autoware_adapi_v1_msgs::msg::ResponseStatus respose_staus; + respose_staus = error.status(); + res->status.success = respose_staus.success; + res->status.code = respose_staus.code; + res->status.message = respose_staus.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 index 623cfe50307f5..462191f57242e 100644 --- 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,7 @@ 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, From 352e4e6d2dcc9fba06ce4c617f06f998c74d0367 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 17 May 2024 17:09:29 +0900 Subject: [PATCH 03/10] modify readme Signed-off-by: Yamato Ando --- localization/pose_initializer/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 7006becf00c2f..19d77c79f1918 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -19,7 +19,7 @@ This node depends on the map height fitter library. | Name | Type | Description | | -------------------------- | --------------------------------------------------- | --------------------- | -| `/localization/initialize` | autoware_adapi_v1_msgs::srv::InitializeLocalization | initial pose from api | +| `/localization/initialize` | tier4_localization_msgs::srv::InitializeLocalization | initial pose from api | ### Clients From c22055124b0f735cfc91fda4fca80727403bc6aa Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 17 May 2024 08:12:12 +0000 Subject: [PATCH 04/10] style(pre-commit): autofix --- .../component_interface_specs/localization.hpp | 2 +- common/component_interface_specs/package.xml | 2 +- localization/pose_initializer/README.md | 4 ++-- .../pose_initializer/pose_initializer_core.cpp | 17 +++++++++++------ .../pose_initializer/pose_initializer_core.hpp | 3 ++- system/default_ad_api/src/localization.hpp | 8 +++----- .../src/utils/localization_conversion.hpp | 6 ++++-- 7 files changed, 24 insertions(+), 18 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 index 0cdd5f7054ca8..e00d8cef1840d 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -18,9 +18,9 @@ #include #include -#include #include #include +#include namespace localization_interface { diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index f9d690d8c9ebf..13f135c925258 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -21,8 +21,8 @@ rclcpp rosidl_runtime_cpp tier4_control_msgs - tier4_map_msgs tier4_localization_msgs + tier4_map_msgs tier4_planning_msgs tier4_system_msgs tier4_vehicle_msgs diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 19d77c79f1918..fcb473db0dd27 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -17,8 +17,8 @@ This node depends on the map height fitter library. ### Services -| Name | Type | Description | -| -------------------------- | --------------------------------------------------- | --------------------- | +| Name | Type | Description | +| -------------------------- | ---------------------------------------------------- | --------------------- | | `/localization/initialize` | tier4_localization_msgs::srv::InitializeLocalization | initial pose from api | ### Clients diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 452c373d5898f..983f315f51ed6 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -22,8 +22,8 @@ #include "stop_check_module.hpp" #include "yabloc_module.hpp" -#include #include +#include #include PoseInitializer::PoseInitializer() : Node("pose_initializer") @@ -115,7 +115,8 @@ void PoseInitializer::change_node_trigger(bool flag, bool need_spin) } } -void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose, bool need_spin) +void PoseInitializer::set_user_defined_initial_pose( + const geometry_msgs::msg::Pose initial_pose, bool need_spin) { try { change_state(State::Message::INITIALIZING); @@ -152,7 +153,8 @@ void PoseInitializer::on_initialize( 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(); + 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_) { @@ -170,9 +172,11 @@ void PoseInitializer::on_initialize( } else if (req->method == Initialize::Service::Request::DIRECT) { if (req->pose_with_covariance.empty()) { std::stringstream message; - message << "No imput pose_with_covariance. If you wanna use DIRECT method, please input pose_with_covariance."; + message << "No imput pose_with_covariance. If you wanna 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()); + 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); @@ -182,7 +186,8 @@ void PoseInitializer::on_initialize( 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()); + throw ServiceException( + autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); } } catch (const ServiceException & error) { diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index 462191f57242e..c7f1c5eec6db0 100644 --- 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, bool need_spin = false); + 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/src/localization.hpp b/system/default_ad_api/src/localization.hpp index 52f804cc1e0b9..a24e2110fabd1 100644 --- a/system/default_ad_api/src/localization.hpp +++ b/system/default_ad_api/src/localization.hpp @@ -37,11 +37,9 @@ class LocalizationNode : public rclcpp::Node Cli cli_initialize_; Sub 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); - + 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.hpp b/system/default_ad_api/src/utils/localization_conversion.hpp index baa3d34676de5..85b6e81e6cd91 100644 --- a/system/default_ad_api/src/utils/localization_conversion.hpp +++ b/system/default_ad_api/src/utils/localization_conversion.hpp @@ -23,8 +23,10 @@ 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; +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; From 98b25f13f98ccb1bb6b0974665a9750cc895687d Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 17 May 2024 17:20:49 +0900 Subject: [PATCH 05/10] delete redundant line Signed-off-by: Yamato Ando --- .../src/pose_initializer/pose_initializer_core.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 983f315f51ed6..8174105add731 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -189,7 +189,6 @@ void PoseInitializer::on_initialize( throw ServiceException( autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); } - } catch (const ServiceException & error) { autoware_adapi_v1_msgs::msg::ResponseStatus respose_staus; respose_staus = error.status(); From e3725ec4f0f6339f76da9bf85bd308390d8063d0 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 17 May 2024 17:25:14 +0900 Subject: [PATCH 06/10] fix typo Signed-off-by: Yamato Ando --- .../src/pose_initializer/pose_initializer_core.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 8174105add731..d4585c966e9cc 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -172,7 +172,7 @@ void PoseInitializer::on_initialize( } else if (req->method == Initialize::Service::Request::DIRECT) { if (req->pose_with_covariance.empty()) { std::stringstream message; - message << "No imput pose_with_covariance. If you wanna use DIRECT method, please input " + message << "No input pose_with_covariance. If you wanna use DIRECT method, please input " "pose_with_covariance."; RCLCPP_ERROR(get_logger(), message.str().c_str()); throw ServiceException( @@ -190,11 +190,11 @@ void PoseInitializer::on_initialize( autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); } } catch (const ServiceException & error) { - autoware_adapi_v1_msgs::msg::ResponseStatus respose_staus; - respose_staus = error.status(); - res->status.success = respose_staus.success; - res->status.code = respose_staus.code; - res->status.message = respose_staus.message; + 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); } } From 3be96ef8f47c0871bbde0b7012c10b331d6dfbf0 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 17 May 2024 18:51:44 +0900 Subject: [PATCH 07/10] Update localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> --- .../src/pose_initializer/pose_initializer_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index d4585c966e9cc..dd368960d33db 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -172,7 +172,7 @@ void PoseInitializer::on_initialize( } 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 wanna use DIRECT method, please input " + 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( From 5c97d9eda10c3fffa0d6acdee83655955a14894b Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 20 May 2024 17:13:35 +0900 Subject: [PATCH 08/10] add description in readme Signed-off-by: Yamato Ando --- localization/pose_initializer/README.md | 80 +++++++++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index fcb473db0dd27..63bbdf3ce49ef 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -46,3 +46,83 @@ 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). drawing + +## Initialize pose via CLI + +### Using the GNSS estimated position + +``` +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 +``` +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 +``` +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 +``` +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. From a20083f6f9a81a05c69070bc2f660b854b0e3395 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 20 May 2024 08:15:48 +0000 Subject: [PATCH 09/10] style(pre-commit): autofix --- localization/pose_initializer/README.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 63bbdf3ce49ef..d4c2206c5f9d1 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -54,10 +54,11 @@ This `pose_initializer` is used via default AD API. For detailed description of ``` 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. +The GNSS estimated position is used as the initial guess, and the localization algorithm automatically estimates a more accurate position. ### Using the input position + ``` ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " pose_with_covariance: @@ -78,9 +79,11 @@ pose_with_covariance: 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 + ``` ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " pose_with_covariance: @@ -101,11 +104,11 @@ pose_with_covariance: method: 1 " ``` -The initial position is set directly by the input position without going through localization algorithm. +The initial position is set directly by the input position without going through localization algorithm. +### Via ros2 topic pub -### Via ros2 topic pub ``` ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped " header: From cf6967961dc178191fe6d636710ae7d401a15888 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 20 May 2024 17:28:14 +0900 Subject: [PATCH 10/10] add bash Signed-off-by: Yamato Ando --- localization/pose_initializer/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index d4c2206c5f9d1..36cc92e1d235b 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -51,7 +51,7 @@ This `pose_initializer` is used via default AD API. For detailed description of ### Using the GNSS estimated position -``` +```bash ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization ``` @@ -59,7 +59,7 @@ The GNSS estimated position is used as the initial guess, and the localization a ### Using the input position -``` +```bash ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " pose_with_covariance: - header: @@ -84,7 +84,7 @@ The input position is used as the initial guess, and the localization algorithm ### Direct initial position set -``` +```bash ros2 service call /localization/initialize tier4_localization_msgs/srv/InitializeLocalization " pose_with_covariance: - header: @@ -109,7 +109,7 @@ The initial position is set directly by the input position without going through ### Via ros2 topic pub -``` +```bash ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped " header: frame_id: map