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..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,16 +18,16 @@ #include #include -#include #include #include +#include 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..13f135c925258 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -21,6 +21,7 @@ rclcpp rosidl_runtime_cpp tier4_control_msgs + tier4_localization_msgs tier4_map_msgs tier4_planning_msgs tier4_system_msgs diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 7006becf00c2f..36cc92e1d235b 100644 --- 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). drawing + +## 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 index bc86b5476dcca..dd368960d33db 100644 --- 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 +#include #include 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 index 623cfe50307f5..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); + 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 index 7975937f5b2c1..799a73508a061 100644 --- 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 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..a24e2110fabd1 100644 --- 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 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..85b6e81e6cd91 --- /dev/null +++ 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 + +#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_