diff --git a/autoware_iv_external_api_adaptor/src/converter/response_status.hpp b/autoware_iv_external_api_adaptor/src/converter/response_status.hpp index 4ec9672..d3531a6 100644 --- a/autoware_iv_external_api_adaptor/src/converter/response_status.hpp +++ b/autoware_iv_external_api_adaptor/src/converter/response_status.hpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace external_api::converter @@ -25,6 +26,7 @@ namespace external_api::converter using AdResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; using T4ResponseStatus = tier4_external_api_msgs::msg::ResponseStatus; +using CommonResponseStatus = autoware_common_msgs::msg::ResponseStatus; inline T4ResponseStatus convert(const AdResponseStatus & ad) { @@ -35,6 +37,15 @@ inline T4ResponseStatus convert(const AdResponseStatus & ad) } } +inline T4ResponseStatus convert(const CommonResponseStatus & common) +{ + if (common.success) { + return tier4_api_utils::response_success(common.message); + } else { + return tier4_api_utils::response_error(common.message); + } +} + } // namespace external_api::converter #endif // CONVERTER__RESPONSE_STATUS_HPP_ diff --git a/autoware_iv_external_api_adaptor/src/initial_pose.cpp b/autoware_iv_external_api_adaptor/src/initial_pose.cpp index 8663ba8..abf0ebe 100644 --- a/autoware_iv_external_api_adaptor/src/initial_pose.cpp +++ b/autoware_iv_external_api_adaptor/src/initial_pose.cpp @@ -61,8 +61,9 @@ void InitialPose::setInitializePose( const tier4_external_api_msgs::srv::InitializePose::Response::SharedPtr response) { const auto req = std::make_shared(); - req->pose.push_back(request->pose); - req->pose.back().pose.covariance = particle_covariance; + req->method = localization_interface::Initialize::Service::Request::AUTO; + req->pose_with_covariance.push_back(request->pose); + req->pose_with_covariance.back().pose.covariance = particle_covariance; try { const auto res = cli_localization_initialize_->call(req, initial_pose_timeout); @@ -77,6 +78,7 @@ void InitialPose::setInitializePoseAuto( const tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr response) { const auto req = std::make_shared(); + req->method = localization_interface::Initialize::Service::Request::AUTO; try { const auto res = cli_localization_initialize_->call(req, initial_pose_timeout);