From 53daf9ab7a33857d869262e2c2d258e8f4a8a32d Mon Sep 17 00:00:00 2001 From: ryohei sasaki Date: Thu, 2 Mar 2023 02:49:14 -0500 Subject: [PATCH] feat(pose_initilizer): support gnss/imu pose estimator (#2904) * Support GNSS/IMU pose estimator Signed-off-by: Ryohei Sasaki * style(pre-commit): autofix Signed-off-by: Ryohei Sasaki * Revert gnss/imu support Signed-off-by: Ryohei Sasaki * Support GNSS/IMU pose estimator Signed-off-by: Ryohei Sasaki * style(pre-commit): autofix Signed-off-by: Ryohei Sasaki * Separate EKF and NDT trigger modules Signed-off-by: Ryohei Sasaki * Integrate activate and deactivate into sendRequest Signed-off-by: Ryohei Sasaki * style(pre-commit): autofix Signed-off-by: Ryohei Sasaki * Change sendRequest function arguments Signed-off-by: Ryohei Sasaki * style(pre-commit): autofix Signed-off-by: Ryohei Sasaki * Remove unused conditional branches Signed-off-by: Ryohei Sasaki * Fix command name Signed-off-by: Ryohei Sasaki * Change to snake_case Signed-off-by: Ryohei Sasaki * Fix typos Signed-off-by: Ryohei Sasaki * Update localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Signed-off-by: Ryohei Sasaki * Update localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Signed-off-by: Ryohei Sasaki * Update copyright year Signed-off-by: Ryohei Sasaki * Set the copyright year of ekf_localization_module to 2022 Signed-off-by: Ryohei Sasaki * Delete unnecessary conditional branches Signed-off-by: Ryohei Sasaki * Add ekf_enabled parameter Signed-off-by: Ryohei Sasaki * Add #include Signed-off-by: Ryohei Sasaki --------- Signed-off-by: Ryohei Sasaki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Ryohei Sasaki Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- .../launch/util/util.launch.xml | 1 + .../launch/simulator.launch.xml | 1 + localization/pose_initializer/CMakeLists.txt | 3 +- localization/pose_initializer/README.md | 1 + .../launch/pose_initializer.launch.xml | 2 + .../ekf_localization_trigger_module.cpp | 56 ++++++++++++++ ...pp => ekf_localization_trigger_module.hpp} | 14 ++-- .../localization_trigger_module.cpp | 75 ------------------- .../ndt_localization_trigger_module.cpp | 56 ++++++++++++++ .../ndt_localization_trigger_module.hpp | 36 +++++++++ .../pose_initializer_core.cpp | 22 ++++-- .../pose_initializer_core.hpp | 6 +- 12 files changed, 181 insertions(+), 92 deletions(-) create mode 100644 localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp rename localization/pose_initializer/src/pose_initializer/{localization_trigger_module.hpp => ekf_localization_trigger_module.hpp} (68%) delete mode 100644 localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp create mode 100644 localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp create mode 100644 localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index 1dfe2e3d5f109..d820e06deb614 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -16,6 +16,7 @@ + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index d48485bc3ff06..aa1877be4f536 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -99,6 +99,7 @@ + diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt index eca7018db8e57..d048cd1cce26f 100644 --- a/localization/pose_initializer/CMakeLists.txt +++ b/localization/pose_initializer/CMakeLists.txt @@ -10,7 +10,8 @@ ament_auto_add_executable(pose_initializer_node src/pose_initializer/gnss_module.cpp src/pose_initializer/ndt_module.cpp src/pose_initializer/stop_check_module.cpp - src/pose_initializer/localization_trigger_module.cpp + src/pose_initializer/ekf_localization_trigger_module.cpp + src/pose_initializer/ndt_localization_trigger_module.cpp ) if(BUILD_TESTING) diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 9b13fda32fa93..81cfe3c4a0665 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -15,6 +15,7 @@ This node depends on the map height fitter library. | Name | Type | Description | | --------------------- | ---- | ---------------------------------------------------------------------------------------- | +| `ekf_enabled` | bool | If true, EKF localizar is activated. | | `ndt_enabled` | bool | If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through. | | `stop_check_enabled` | bool | If true, initialization is accepted only when the vehicle is stopped. | | `stop_check_duration` | bool | The duration used for the stop check above. | diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 7d73cf26b4766..9483ff47bc093 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -2,10 +2,12 @@ + + diff --git a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp new file mode 100644 index 0000000000000..ac9796b687637 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp @@ -0,0 +1,56 @@ +// 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. +// 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 "ekf_localization_trigger_module.hpp" + +#include +#include + +#include +#include + +using ServiceException = component_interface_utils::ServiceException; +using Initialize = localization_interface::Initialize; + +EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node) +: logger_(node->get_logger()) +{ + client_ekf_trigger_ = node->create_client("ekf_trigger_node"); +} + +void EkfLocalizationTriggerModule::send_request(bool flag) const +{ + const auto req = std::make_shared(); + std::string command_name; + req->data = flag; + if (flag) { + command_name = "Activation"; + } else { + command_name = "Deactivation"; + } + + if (!client_ekf_trigger_->service_is_ready()) { + throw component_interface_utils::ServiceUnready("EKF triggering service is not ready"); + } + + auto future_ekf = client_ekf_trigger_->async_send_request(req); + + if (future_ekf.get()->success) { + RCLCPP_INFO(logger_, "EKF %s succeeded", command_name.c_str()); + } else { + RCLCPP_INFO(logger_, "EKF %s failed", command_name.c_str()); + throw ServiceException( + Initialize::Service::Response::ERROR_ESTIMATION, "EKF " + command_name + " failed"); + } +} diff --git a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp similarity index 68% rename from localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp rename to localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp index 3e32518c89698..d1b8eb986105f 100644 --- a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp @@ -12,27 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_ -#define POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_ +#ifndef POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_ +#define POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_ #include #include -class LocalizationTriggerModule +class EkfLocalizationTriggerModule { private: using SetBool = std_srvs::srv::SetBool; public: - explicit LocalizationTriggerModule(rclcpp::Node * node); - void deactivate() const; - void activate() const; + explicit EkfLocalizationTriggerModule(rclcpp::Node * node); + void send_request(bool flag) const; private: rclcpp::Logger logger_; rclcpp::Client::SharedPtr client_ekf_trigger_; - rclcpp::Client::SharedPtr client_ndt_trigger_; }; -#endif // POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_ +#endif // POSE_INITIALIZER__EKF_LOCALIZATION_TRIGGER_MODULE_HPP_ diff --git a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp deleted file mode 100644 index b197eea3f1d9a..0000000000000 --- a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// 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. -// 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.S -#include "localization_trigger_module.hpp" - -#include -#include - -#include - -using ServiceException = component_interface_utils::ServiceException; -using Initialize = localization_interface::Initialize; - -LocalizationTriggerModule::LocalizationTriggerModule(rclcpp::Node * node) -: logger_(node->get_logger()) -{ - client_ekf_trigger_ = node->create_client("ekf_trigger_node"); - client_ndt_trigger_ = node->create_client("ndt_trigger_node"); -} - -void LocalizationTriggerModule::deactivate() const -{ - const auto req = std::make_shared(); - req->data = false; - - if (!client_ekf_trigger_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("EKF triggering service is not ready"); - } - if (!client_ndt_trigger_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("NDT triggering service is not ready"); - } - - auto future_ekf = client_ekf_trigger_->async_send_request(req); - auto future_ndt = client_ndt_trigger_->async_send_request(req); - - if (future_ekf.get()->success & future_ndt.get()->success) { - RCLCPP_INFO(logger_, "Deactivation succeeded"); - } else { - RCLCPP_INFO(logger_, "Deactivation failed"); - throw ServiceException(Initialize::Service::Response::ERROR_ESTIMATION, "Deactivation failed"); - } -} - -void LocalizationTriggerModule::activate() const -{ - const auto req = std::make_shared(); - req->data = true; - - if (!client_ekf_trigger_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("EKF triggering service is not ready"); - } - if (!client_ndt_trigger_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("NDT triggering service is not ready"); - } - - auto future_ekf = client_ekf_trigger_->async_send_request(req); - auto future_ndt = client_ndt_trigger_->async_send_request(req); - - if (future_ekf.get()->success & future_ndt.get()->success) { - RCLCPP_INFO(logger_, "Activation succeeded"); - } else { - RCLCPP_INFO(logger_, "Activation failed"); - throw ServiceException(Initialize::Service::Response::ERROR_ESTIMATION, "Activation failed"); - } -} diff --git a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp new file mode 100644 index 0000000000000..e1285f5c31c83 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp @@ -0,0 +1,56 @@ +// Copyright 2023 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. +// 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 "ndt_localization_trigger_module.hpp" + +#include +#include + +#include +#include + +using ServiceException = component_interface_utils::ServiceException; +using Initialize = localization_interface::Initialize; + +NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node) +: logger_(node->get_logger()) +{ + client_ndt_trigger_ = node->create_client("ndt_trigger_node"); +} + +void NdtLocalizationTriggerModule::send_request(bool flag) const +{ + const auto req = std::make_shared(); + std::string command_name; + req->data = flag; + if (flag) { + command_name = "Activation"; + } else { + command_name = "Deactivation"; + } + + if (!client_ndt_trigger_->service_is_ready()) { + throw component_interface_utils::ServiceUnready("NDT triggering service is not ready"); + } + + auto future_ndt = client_ndt_trigger_->async_send_request(req); + + if (future_ndt.get()->success) { + RCLCPP_INFO(logger_, "NDT %s succeeded", command_name.c_str()); + } else { + RCLCPP_INFO(logger_, "NDT %s failed", command_name.c_str()); + throw ServiceException( + Initialize::Service::Response::ERROR_ESTIMATION, "NDT " + command_name + " failed"); + } +} diff --git a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp new file mode 100644 index 0000000000000..91e37c9bb90e9 --- /dev/null +++ b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp @@ -0,0 +1,36 @@ +// Copyright 2023 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. +// 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 POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_ +#define POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_ + +#include + +#include + +class NdtLocalizationTriggerModule +{ +private: + using SetBool = std_srvs::srv::SetBool; + +public: + explicit NdtLocalizationTriggerModule(rclcpp::Node * node); + void send_request(bool flag) const; + +private: + rclcpp::Logger logger_; + rclcpp::Client::SharedPtr client_ndt_trigger_; +}; + +#endif // POSE_INITIALIZER__NDT_LOCALIZATION_TRIGGER_MODULE_HPP_ 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 7222ac2c4a244..e4f590a7b1f42 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -15,8 +15,9 @@ #include "pose_initializer_core.hpp" #include "copy_vector_to_array.hpp" +#include "ekf_localization_trigger_module.hpp" #include "gnss_module.hpp" -#include "localization_trigger_module.hpp" +#include "ndt_localization_trigger_module.hpp" #include "ndt_module.hpp" #include "stop_check_module.hpp" @@ -34,12 +35,15 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); + if (declare_parameter("ekf_enabled")) { + ekf_localization_trigger_ = std::make_unique(this); + } if (declare_parameter("gnss_enabled")) { gnss_ = std::make_unique(this); } if (declare_parameter("ndt_enabled")) { ndt_ = std::make_unique(this); - localization_trigger_ = std::make_unique(this); + ndt_localization_trigger_ = std::make_unique(this); } if (declare_parameter("stop_check_enabled")) { // Add 1.0 sec margin for twist buffer. @@ -73,8 +77,11 @@ void PoseInitializer::on_initialize( } try { change_state(State::Message::INITIALIZING); - if (localization_trigger_) { - localization_trigger_->deactivate(); + if (ekf_localization_trigger_) { + ekf_localization_trigger_->send_request(false); + } + if (ndt_localization_trigger_) { + ndt_localization_trigger_->send_request(false); } auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front(); if (ndt_) { @@ -82,8 +89,11 @@ void PoseInitializer::on_initialize( } pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); - if (localization_trigger_) { - localization_trigger_->activate(); + if (ekf_localization_trigger_) { + ekf_localization_trigger_->send_request(true); + } + if (ndt_localization_trigger_) { + ndt_localization_trigger_->send_request(true); } res->status.success = true; change_state(State::Message::INITIALIZED); 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 a80ed5ea954ea..5fb6677b6b71f 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -26,7 +26,8 @@ class StopCheckModule; class NdtModule; class GnssModule; -class LocalizationTriggerModule; +class EkfLocalizationTriggerModule; +class NdtLocalizationTriggerModule; class PoseInitializer : public rclcpp::Node { @@ -50,7 +51,8 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr gnss_; std::unique_ptr ndt_; std::unique_ptr stop_check_; - std::unique_ptr localization_trigger_; + std::unique_ptr ekf_localization_trigger_; + std::unique_ptr ndt_localization_trigger_; double stop_check_duration_; void change_state(State::Message::_state_type state); void on_initialize(