diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 92df806605357..aa412fab317a2 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -66,6 +66,7 @@ #include #include #include +#include #include class NDTScanMatcher : public rclcpp::Node @@ -105,7 +106,7 @@ class NDTScanMatcher : public rclcpp::Node const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); - geometry_msgs::msg::PoseWithCovarianceStamped align_pose( + std::tuple align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); void transform_sensor_measurement( diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a90ec30aa91ff..3e8b9b99737c0 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -994,12 +994,22 @@ void NDTScanMatcher::service_ndt_align_main( return; } - res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame); + // estimate initial pose + const auto [pose_with_covariance, score] = align_pose(initial_pose_msg_in_map_frame); + + // check reliability of initial pose result + res->reliable = + (param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood < score); + if (!res->reliable) { + RCLCPP_WARN_STREAM( + this->get_logger(), "Initial Pose Estimation is Unstable. Score is " << score); + } res->success = true; + res->pose_with_covariance = pose_with_covariance; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; } -geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( +std::tuple NDTScanMatcher::align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); @@ -1089,13 +1099,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( std::begin(particle_array), std::end(particle_array), [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); - if ( - best_particle_ptr->score < - param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood) - RCLCPP_WARN_STREAM( - this->get_logger(), - "Initial Pose Estimation is Unstable. Score is " << best_particle_ptr->score); - geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; @@ -1104,7 +1107,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score); - return result_pose_with_cov_msg; + return std::make_tuple(result_pose_with_cov_msg, best_particle_ptr->score); } #include diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 36cc92e1d235b..9083a6570d33b 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -41,6 +41,14 @@ This node depends on the map height fitter library. | `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state | | `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | +## Diagnostics + +### pose_initializer_status + +If the score of initial pose estimation result is lower than score threshold, ERROR message is output to the `/diagnostics` topic. + +drawing + ## Connection with Default AD API 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). diff --git a/localization/pose_initializer/media/diagnostic_pose_reliability.png b/localization/pose_initializer/media/diagnostic_pose_reliability.png new file mode 100644 index 0000000000000..7e2d8dc5e632a Binary files /dev/null and b/localization/pose_initializer/media/diagnostic_pose_reliability.png differ diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 5f3ea72f7d400..ca4aa032fc7f9 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -24,6 +24,7 @@ component_interface_specs component_interface_utils geometry_msgs + localization_util map_height_fitter rclcpp rclcpp_components diff --git a/localization/pose_initializer/src/pose_initializer/localization_module.cpp b/localization/pose_initializer/src/pose_initializer/localization_module.cpp index 433833cd15784..6963e5adbba3e 100644 --- a/localization/pose_initializer/src/pose_initializer/localization_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/localization_module.cpp @@ -30,7 +30,8 @@ LocalizationModule::LocalizationModule(rclcpp::Node * node, const std::string & cli_align_ = node->create_client(service_name); } -PoseWithCovarianceStamped LocalizationModule::align_pose(const PoseWithCovarianceStamped & pose) +std::tuple LocalizationModule::align_pose( + const PoseWithCovarianceStamped & pose) { const auto req = std::make_shared(); req->pose_with_covariance = pose; @@ -47,5 +48,5 @@ PoseWithCovarianceStamped LocalizationModule::align_pose(const PoseWithCovarianc RCLCPP_INFO(logger_, "align server succeeded."); // Overwrite the covariance. - return res->pose_with_covariance; + return std::make_tuple(res->pose_with_covariance, res->reliable); } diff --git a/localization/pose_initializer/src/pose_initializer/localization_module.hpp b/localization/pose_initializer/src/pose_initializer/localization_module.hpp index f17f68670306c..dc8bd49a26a79 100644 --- a/localization/pose_initializer/src/pose_initializer/localization_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/localization_module.hpp @@ -21,6 +21,7 @@ #include #include +#include class LocalizationModule { @@ -30,7 +31,7 @@ class LocalizationModule public: LocalizationModule(rclcpp::Node * node, const std::string & service_name); - PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose); + std::tuple align_pose(const PoseWithCovarianceStamped & pose); private: rclcpp::Logger logger_; 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 6e023a1309c2d..6535863829fac 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -37,6 +37,8 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); + diagnostics_pose_reliable_ = std::make_unique(this, "pose_initializer_status"); + if (declare_parameter("ekf_enabled")) { ekf_localization_trigger_ = std::make_unique(this); } @@ -151,13 +153,27 @@ void PoseInitializer::on_initialize( auto pose = req->pose_with_covariance.empty() ? get_gnss_pose() : req->pose_with_covariance.front(); + bool reliable = true; if (ndt_) { - pose = ndt_->align_pose(pose); + std::tie(pose, reliable) = 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); + std::tie(pose, reliable) = yabloc_->align_pose(pose); + } + + diagnostics_pose_reliable_->clear(); + + // check initial pose result and publish diagnostics + diagnostics_pose_reliable_->add_key_value("initial_pose_reliable", reliable); + if (!reliable) { + std::stringstream message; + message << "Initial Pose Estimation is Unstable."; + diagnostics_pose_reliable_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); } + diagnostics_pose_reliable_->publish(this->now()); + pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); 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 0a9a0614f6d20..cd82d522187c2 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -15,6 +15,8 @@ #ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ +#include "localization_util/diagnostics_module.hpp" + #include #include #include @@ -55,6 +57,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index 6aff744ef4975..23f711d1ba17b 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -199,6 +199,8 @@ void CameraPoseInitializer::on_service( // Estimate orientation const double initial_yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w); const auto yaw_angle_rad_opt = estimate_pose(pos_vec3f, initial_yaw_angle_rad, yaw_std_rad); + // TODO(localization/mapping team): judge reliability of estimate pose + response->reliable = true; if (yaw_angle_rad_opt.has_value()) { response->success = true; response->pose_with_covariance =