diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 7f2f446ae1bee..cfc09459a5cb1 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -2,6 +2,7 @@ + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 3042774d16fd3..c09443be8cab6 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -21,6 +21,11 @@ + + + + + @@ -91,6 +96,8 @@ + + diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index 88acbfb52611d..90ec78257e6cb 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -1,5 +1,9 @@ /**: ros__parameters: + user_defined_initial_pose: + enable: $(var user_defined_initial_pose/enable) + pose: $(var user_defined_initial_pose/pose) + gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] ekf_enabled: $(var ekf_enabled) diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json index ced4dc78b93b1..d5b92c45d3038 100644 --- a/localization/pose_initializer/schema/pose_initializer.schema.json +++ b/localization/pose_initializer/schema/pose_initializer.schema.json @@ -6,6 +6,23 @@ "pose_initializer": { "type": "object", "properties": { + "user_defined_initial_pose": { + "type": "object", + "properties": { + "enable": { + "type": "string", + "description": "If true, user_defined_initial_pose.pose is set as the initial position. [boolean]", + "default": "false" + }, + "pose": { + "type": "string", + "description": "initial pose (x, y, z, quat_x, quat_y, quat_z, quat_w). [array]", + "default": "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]" + } + }, + "required": ["enable", "pose"], + "additionalProperties": false + }, "gnss_pose_timeout": { "type": "number", "description": "The duration that the GNSS pose is valid. [sec]", @@ -55,6 +72,7 @@ } }, "required": [ + "user_defined_initial_pose", "gnss_pose_timeout", "stop_check_duration", "ekf_enabled", 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 index ac9796b687637..9ba424f8e857f 100644 --- a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.cpp @@ -23,13 +23,20 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; -EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node) -: logger_(node->get_logger()) +EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node) : node_(node) { - client_ekf_trigger_ = node->create_client("ekf_trigger_node"); + client_ekf_trigger_ = node_->create_client("ekf_trigger_node"); } -void EkfLocalizationTriggerModule::send_request(bool flag) const +void EkfLocalizationTriggerModule::wait_for_service() +{ + while (!client_ekf_trigger_->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_INFO(node_->get_logger(), "EKF triggering service is not available, waiting..."); + } + RCLCPP_INFO(node_->get_logger(), "EKF triggering service is available!"); +} + +void EkfLocalizationTriggerModule::send_request(bool flag, bool need_spin) const { const auto req = std::make_shared(); std::string command_name; @@ -46,10 +53,14 @@ void EkfLocalizationTriggerModule::send_request(bool flag) const auto future_ekf = client_ekf_trigger_->async_send_request(req); + if (need_spin) { + rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future_ekf); + } + if (future_ekf.get()->success) { - RCLCPP_INFO(logger_, "EKF %s succeeded", command_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "EKF %s succeeded", command_name.c_str()); } else { - RCLCPP_INFO(logger_, "EKF %s failed", command_name.c_str()); + RCLCPP_INFO(node_->get_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/ekf_localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp index d1b8eb986105f..901e4ec4414b5 100644 --- a/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/ekf_localization_trigger_module.hpp @@ -26,10 +26,11 @@ class EkfLocalizationTriggerModule public: explicit EkfLocalizationTriggerModule(rclcpp::Node * node); - void send_request(bool flag) const; + void wait_for_service(); + void send_request(bool flag, bool need_spin = false) const; private: - rclcpp::Logger logger_; + rclcpp::Node * node_; rclcpp::Client::SharedPtr client_ekf_trigger_; }; 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 index e1285f5c31c83..436b1e2df3b21 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.cpp @@ -23,13 +23,20 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; -NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node) -: logger_(node->get_logger()) +NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node) : node_(node) { - client_ndt_trigger_ = node->create_client("ndt_trigger_node"); + client_ndt_trigger_ = node_->create_client("ndt_trigger_node"); } -void NdtLocalizationTriggerModule::send_request(bool flag) const +void NdtLocalizationTriggerModule::wait_for_service() +{ + while (!client_ndt_trigger_->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_INFO(node_->get_logger(), "NDT triggering service is not available, waiting..."); + } + RCLCPP_INFO(node_->get_logger(), "NDT triggering service is available!"); +} + +void NdtLocalizationTriggerModule::send_request(bool flag, bool need_spin) const { const auto req = std::make_shared(); std::string command_name; @@ -46,10 +53,14 @@ void NdtLocalizationTriggerModule::send_request(bool flag) const auto future_ndt = client_ndt_trigger_->async_send_request(req); + if (need_spin) { + rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future_ndt); + } + if (future_ndt.get()->success) { - RCLCPP_INFO(logger_, "NDT %s succeeded", command_name.c_str()); + RCLCPP_INFO(node_->get_logger(), "NDT %s succeeded", command_name.c_str()); } else { - RCLCPP_INFO(logger_, "NDT %s failed", command_name.c_str()); + RCLCPP_INFO(node_->get_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 index 91e37c9bb90e9..1c820557fb8ff 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_localization_trigger_module.hpp @@ -26,10 +26,11 @@ class NdtLocalizationTriggerModule public: explicit NdtLocalizationTriggerModule(rclcpp::Node * node); - void send_request(bool flag) const; + void wait_for_service(); + void send_request(bool flag, bool need_spin = false) const; private: - rclcpp::Logger logger_; + rclcpp::Node * node_; rclcpp::Client::SharedPtr client_ndt_trigger_; }; 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 4585414c20b0d..bc86b5476dcca 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -57,6 +57,31 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); + + if (declare_parameter("user_defined_initial_pose.enable")) { + const auto initial_pose_array = + declare_parameter>("user_defined_initial_pose.pose"); + if (initial_pose_array.size() != 7) { + throw std::invalid_argument( + "Could not set user defined initial pose. The size of initial_pose is " + + std::to_string(initial_pose_array.size()) + ". It must be 7."); + } else if ( + std::abs(initial_pose_array[3]) < 1e-6 && std::abs(initial_pose_array[4]) < 1e-6 && + std::abs(initial_pose_array[5]) < 1e-6 && std::abs(initial_pose_array[6]) < 1e-6) { + throw std::invalid_argument("Input quaternion is invalid. All elements are close to zero."); + } else { + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = initial_pose_array[0]; + initial_pose.position.y = initial_pose_array[1]; + initial_pose.position.z = initial_pose_array[2]; + initial_pose.orientation.x = initial_pose_array[3]; + initial_pose.orientation.y = initial_pose_array[4]; + initial_pose.orientation.z = initial_pose_array[5]; + initial_pose.orientation.w = initial_pose_array[6]; + + set_user_defined_initial_pose(initial_pose); + } + } } PoseInitializer::~PoseInitializer() @@ -71,6 +96,47 @@ void PoseInitializer::change_state(State::Message::_state_type state) pub_state_->publish(state_); } +// To execute in the constructor, you need to call ros spin. +// Conversely, ros spin should not be called elsewhere +void PoseInitializer::change_node_trigger(bool flag, bool need_spin) +{ + try { + if (ekf_localization_trigger_) { + ekf_localization_trigger_->wait_for_service(); + ekf_localization_trigger_->send_request(flag, need_spin); + } + if (ndt_localization_trigger_) { + ndt_localization_trigger_->wait_for_service(); + ndt_localization_trigger_->send_request(flag, need_spin); + } + } catch (const ServiceException & error) { + throw; + } +} + +void PoseInitializer::set_user_defined_initial_pose(const geometry_msgs::msg::Pose initial_pose) +{ + try { + change_state(State::Message::INITIALIZING); + change_node_trigger(false, true); + + PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = now(); + pose.pose.pose = initial_pose; + pose.pose.covariance = output_pose_covariance_; + pub_reset_->publish(pose); + + change_node_trigger(true, true); + change_state(State::Message::INITIALIZED); + + RCLCPP_INFO(get_logger(), "Set user defined initial pose"); + } catch (const ServiceException & error) { + change_state(State::Message::UNINITIALIZED); + RCLCPP_WARN(get_logger(), "Could not set user defined initial pose"); + } +} + void PoseInitializer::on_initialize( const Initialize::Service::Request::SharedPtr req, const Initialize::Service::Response::SharedPtr res) @@ -82,12 +148,8 @@ void PoseInitializer::on_initialize( } try { change_state(State::Message::INITIALIZING); - if (ekf_localization_trigger_) { - ekf_localization_trigger_->send_request(false); - } - if (ndt_localization_trigger_) { - ndt_localization_trigger_->send_request(false); - } + change_node_trigger(false, false); + auto pose = req->pose.empty() ? get_gnss_pose() : req->pose.front(); if (ndt_) { pose = ndt_->align_pose(pose); @@ -98,12 +160,8 @@ void PoseInitializer::on_initialize( } pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); - if (ekf_localization_trigger_) { - ekf_localization_trigger_->send_request(true); - } - if (ndt_localization_trigger_) { - ndt_localization_trigger_->send_request(true); - } + + change_node_trigger(true, false); res->status.success = true; change_state(State::Message::INITIALIZED); } 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 014c8e9bf6e04..623cfe50307f5 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -58,6 +58,9 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; 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 change_state(State::Message::_state_type state); void on_initialize( const Initialize::Service::Request::SharedPtr req,