diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 4806838cf0103..7ac78514c49fe 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -27,10 +27,11 @@ find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) ament_auto_add_executable(ndt_scan_matcher - src/particle.cpp - src/ndt_scan_matcher_node.cpp - src/ndt_scan_matcher_core.cpp + src/diagnostics_module.cpp src/map_update_module.cpp + src/ndt_scan_matcher_core.cpp + src/ndt_scan_matcher_node.cpp + src/particle.cpp ) link_directories(${PCL_LIBRARY_DIRS}) @@ -45,15 +46,19 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_auto_add_gtest(standard_sequence_for_initial_pose_estimation test/test_cases/standard_sequence_for_initial_pose_estimation.cpp - src/particle.cpp - src/ndt_scan_matcher_core.cpp + src/diagnostics_module.cpp src/map_update_module.cpp + src/ndt_scan_matcher_core.cpp +# src/ndt_scan_matcher_node.cpp + src/particle.cpp ) ament_auto_add_gtest(once_initialize_at_out_of_map_then_initialize_correctly test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp - src/particle.cpp - src/ndt_scan_matcher_core.cpp + src/diagnostics_module.cpp src/map_update_module.cpp + src/ndt_scan_matcher_core.cpp +# src/ndt_scan_matcher_node.cpp + src/particle.cpp ) endif() diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 424a7051de883..fbc2fa3c9a3f3 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -255,3 +255,103 @@ initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of th initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} + +## Diagnostics + +### scan_matching_status + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | +| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -------------------------------- | ------------------------------------------------------------------------ | +| `topic_time_stamp` | the time stamp of input topic | none | none | no | +| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | +| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | +| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | +| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | +| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | yes | +| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | +| `is_set_map_points` | whether the map points is set or not | not set | none | yes | +| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | +| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | +| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | +| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | +| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | +| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | none | the number of times is 5 or more | - | + +※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. + +### initial_pose_subscriber_status + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| ---------------------- | ------------------------------------------------------------------ | ------------------------------- | ----------------------------- | +| `topic_time_stamp` | the time stamp of input topic | none | none | +| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | +| `is_expected_frame_id` | whether the input frame_id is the same as `frame.map_frame` or not | none | not the same | + +### regularization_pose_subscriber_status + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| ------------------ | ----------------------------- | ------------------------------- | ----------------------------- | +| `topic_time_stamp` | the time stamp of input topic | none | none | + +### trigger_node_service_status + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| ------------------------- | -------------------------------------------------- | ------------------------------- | ----------------------------- | +| `service_call_time_stamp` | the time stamp of service calling | none | none | +| `is_activated` | whether the node is in the "activate" state or not | none | none | +| `is_succeed_service` | whether the process of service is succeed or not | none | none | + +※ +This diagnostic is only published when the service is called, so it becomes stale after the initial pose estimation is completed. + +### ndt_align_service_status + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| ----------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------- | ----------------------------------------------------------- | +| `service_call_time_stamp` | the time stamp of service calling | none | none | +| `is_succeed_transform_initial_pose` | whether transform initial pose is succeed or not | none | failed | +| `is_need_rebuild` | whether it need to rebuild the map. If the map has not been loaded yet or if `distance_last_update_position_to_current_position encounters` is an Error state, it is considered necessary to reconstruct the map, and `is_need_rebuild` becomes `True`. | none | none | +| `maps_size_before` | the number of maps before update map | none | none | +| `is_succeed_call_pcd_loader` | whether call pcd_loader service is succeed or not | failed | none | +| `maps_to_add_size` | the number of maps to be added | none | none | +| `maps_to_remove_size` | the number of maps to be removed | none | none | +| `map_update_execution_time` | the time for map updating | none | none | +| `maps_size_after` | the number of maps after update map | none | none | +| `is_updated_map` | whether map is updated. If the map update couldn't be performed or there was no need to update the map, it becomes `False` | none | `is_updated_map` is `False` but `is_need_rebuild` is `True` | +| `is_set_map_points` | whether the map points is set or not | not set | none | +| `is_set_sensor_points` | whether the sensor points is set or not | not set | none | +| `best_particle_score` | the best score of particle | none | none | +| `is_succeed_service` | whether the process of service is succeed or not | failed | none | + +※ +This diagnostic is only published when the service is called, so it becomes stale after the initial pose estimation is completed. + +### map_update_status + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| --------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------------------------------------------------------------- | +| `timer_callback_time_stamp` | the time stamp of timer_callback calling | none | none | +| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | +| `is_set_last_update_position` | whether the `last_update_position` is set or not | not set | none | +| `distance_last_update_position_to_current_position` | the distance of `last_update_position` to current position | none | (the distance + `dynamic_map_loading.lidar_radius`) is **larger** than `dynamic_map_loading.map_radius` | +| `is_need_rebuild` | whether it need to rebuild the map. If the map has not been loaded yet or if `distance_last_update_position_to_current_position encounters` is an Error state, it is considered necessary to reconstruct the map, and `is_need_rebuild` becomes `True`. | none | none | +| `maps_size_before` | the number of maps before update map | none | none | +| `is_succeed_call_pcd_loader` | whether call pcd_loader service is succeed or not | failed | none | +| `maps_to_add_size` | the number of maps to be added | none | none | +| `maps_to_remove_size` | the number of maps to be removed | none | none | +| `map_update_execution_time` | the time for map updating | none | none | +| `maps_size_after` | the number of maps after update map | none | none | +| `is_updated_map` | whether map is updated. If the map update couldn't be performed or there was no need to update the map, it becomes `False` | none | `is_updated_map` is `False` but `is_need_rebuild` is `True` | diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp new file mode 100644 index 0000000000000..828eb6bed346b --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 Autoware Foundation +// +// 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 NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_ +#define NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +class DiagnosticsModule +{ +public: + DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + void clear(); + void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + template + void addKeyValue(const std::string & key, const T & value); + void updateLevelAndMessage(const int8_t level, const std::string & message); + void publish(); + +private: + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray() const; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = std::to_string(value); + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); + +#endif // NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index ee1e2369c79bd..bc7bf1fe40d36 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -162,10 +163,11 @@ struct HyperParameters initial_pose_offset_model_y[i]; } } else { - RCLCPP_WARN( - node->get_logger(), - "Invalid initial pose offset model parameters. Disable covariance estimation."); - covariance.covariance_estimation.enable = false; + std::stringstream message; + message << "Invalid initial pose offset model parameters." + << "Please make sure that the number of elements in " + << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same."; + throw std::runtime_error(message.str()); } } diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 04c762ae950a4..2bcb6f4cf6fb8 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -16,6 +16,7 @@ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #include "localization_util/util_func.hpp" +#include "ndt_scan_matcher/diagnostics_module.hpp" #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/particle.hpp" @@ -54,10 +55,20 @@ class MapUpdateModule private: friend class NDTScanMatcher; + void callback_timer( + const bool is_activated, const std::optional & position, + std::unique_ptr & diagnostics_ptr); + + [[nodiscard]] bool should_update_map( + const geometry_msgs::msg::Point & position, + std::unique_ptr & diagnostics_ptr); + void update_map( + const geometry_msgs::msg::Point & position, + std::unique_ptr & diagnostics_ptr); // Update the specified NDT - bool update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt); - void update_map(const geometry_msgs::msg::Point & position); - [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position); + bool update_ndt( + const geometry_msgs::msg::Point & position, NdtType & ndt, + std::unique_ptr & diagnostics_ptr); void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; 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 26c96ce7e3248..4f8042d106c75 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 @@ -18,6 +18,7 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" +#include "ndt_scan_matcher/diagnostics_module.hpp" #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/map_update_module.hpp" @@ -62,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -77,21 +79,32 @@ class NDTScanMatcher : public rclcpp::Node explicit NDTScanMatcher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: - void service_ndt_align( - const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, - tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); - void service_trigger_node( - const std_srvs::srv::SetBool::Request::SharedPtr req, - std_srvs::srv::SetBool::Response::SharedPtr res); - void callback_timer(); - void callback_sensor_points( - sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame); + void callback_initial_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr); + void callback_initial_pose_main( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr); + void callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); + void callback_sensor_points( + sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame); + bool callback_sensor_points_main( + sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame); + + void service_trigger_node( + const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res); + + void service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); + void service_ndt_align_main( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); + geometry_msgs::msg::PoseWithCovarianceStamped align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); @@ -116,11 +129,6 @@ class NDTScanMatcher : public rclcpp::Node const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg); - bool validate_num_iteration(const int iter_num, const int max_iter_num); - bool validate_score( - const double score, const double score_threshold, const std::string & score_name); - bool validate_converged_param( - const double & transform_probability, const double & nearest_voxel_transformation_likelihood); static int count_oscillation(const std::vector & result_pose_msg_array); std::array rotate_covariance( @@ -131,8 +139,6 @@ class NDTScanMatcher : public rclcpp::Node void add_regularization_pose(const rclcpp::Time & sensor_ros_time); - void publish_diagnostic(); - rclcpp::TimerBase::SharedPtr map_update_timer_; rclcpp::Subscription::SharedPtr initial_pose_sub_; rclcpp::Subscription::SharedPtr sensor_points_sub_; @@ -168,7 +174,6 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Publisher::SharedPtr ndt_marker_pub_; rclcpp::Publisher::SharedPtr ndt_monte_carlo_initial_pose_marker_pub_; - rclcpp::Publisher::SharedPtr diagnostics_pub_; rclcpp::Service::SharedPtr service_; rclcpp::Service::SharedPtr service_trigger_node_; @@ -180,7 +185,6 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr timer_callback_group_; std::shared_ptr ndt_ptr_; - std::shared_ptr> state_ptr_; Eigen::Matrix4f base_to_sensor_matrix_; @@ -194,6 +198,12 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; + std::unique_ptr diagnostics_scan_points_; + std::unique_ptr diagnostics_initial_pose_; + std::unique_ptr diagnostics_regularization_pose_; + std::unique_ptr diagnostics_map_update_; + std::unique_ptr diagnostics_ndt_align_; + std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; diff --git a/localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png b/localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png new file mode 100644 index 0000000000000..766888359ed8e Binary files /dev/null and b/localization/ndt_scan_matcher/media/diagnostic_initial_pose_subscriber_status.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_map_update_status.png b/localization/ndt_scan_matcher/media/diagnostic_map_update_status.png new file mode 100644 index 0000000000000..0fe7016fe7e3c Binary files /dev/null and b/localization/ndt_scan_matcher/media/diagnostic_map_update_status.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png b/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png new file mode 100644 index 0000000000000..e1da17966080f Binary files /dev/null and b/localization/ndt_scan_matcher/media/diagnostic_ndt_align_service_status.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png b/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png new file mode 100644 index 0000000000000..6d777ff360481 Binary files /dev/null and b/localization/ndt_scan_matcher/media/diagnostic_regularization_pose_subscriber_status.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png b/localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png new file mode 100644 index 0000000000000..2687db23d4714 Binary files /dev/null and b/localization/ndt_scan_matcher/media/diagnostic_scan_matching_status.png differ diff --git a/localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png b/localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png new file mode 100644 index 0000000000000..178e2a8bf361f Binary files /dev/null and b/localization/ndt_scan_matcher/media/diagnostic_trigger_node_service_status.png differ diff --git a/localization/ndt_scan_matcher/src/diagnostics_module.cpp b/localization/ndt_scan_matcher/src/diagnostics_module.cpp new file mode 100644 index 0000000000000..9d7eed46414ea --- /dev/null +++ b/localization/ndt_scan_matcher/src/diagnostics_module.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 Autoware Foundation +// +// 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_scan_matcher/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +{ + auto it = std::find_if( + std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value ? "True" : "False"; + addKeyValue(key_value); +} + +void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish() +{ + diagnostics_pub_->publish(createDiagnosticsArray()); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray() const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = clock_->now(); + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index ca56a93cec859..43174a39b797e 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -34,7 +34,10 @@ MapUpdateModule::MapUpdateModule( if (ndt_ptr_) { *secondary_ndt_ptr_ = *ndt_ptr_; } else { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer."); + std::stringstream message; + message << "Error at MapUpdateModule::MapUpdateModule." + << "`ndt_ptr_` is a null NDT pointer."; + throw std::runtime_error(message.str()); } // Initially, a direct map update on ndt_ptr_ is needed. @@ -45,7 +48,41 @@ MapUpdateModule::MapUpdateModule( need_rebuild_ = true; } -bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) +void MapUpdateModule::callback_timer( + const bool is_activated, const std::optional & position, + std::unique_ptr & diagnostics_ptr) +{ + diagnostics_ptr->addKeyValue("timer_callback_time_stamp", clock_->now().seconds()); + + // check is_activated + diagnostics_ptr->addKeyValue("is_activated", is_activated); + if (!is_activated) { + std::stringstream message; + message << "Node is not activated."; + diagnostics_ptr->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + // check is_set_last_update_position + const bool is_set_last_update_position = (position != std::nullopt); + diagnostics_ptr->addKeyValue("is_set_last_update_position", is_set_last_update_position); + if (!is_set_last_update_position) { + std::stringstream message; + message << "Cannot find the reference position for map update." + << "Please check if the EKF odometry is provided to NDT."; + diagnostics_ptr->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; + } + + if (should_update_map(position.value(), diagnostics_ptr)) { + update_map(position.value(), diagnostics_ptr); + } +} + +bool MapUpdateModule::should_update_map( + const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) { if (last_update_position_ == std::nullopt) { need_rebuild_ = true; @@ -55,8 +92,15 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi const double dx = position.x - last_update_position_.value().x; const double dy = position.y - last_update_position_.value().y; const double distance = std::hypot(dx, dy); + + // check distance_last_update_position_to_current_position + diagnostics_ptr->addKeyValue("distance_last_update_position_to_current_position", distance); if (distance + param_.lidar_radius > param_.map_radius) { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); + std::stringstream message; + message << "Dynamic map loading is not keeping up."; + diagnostics_ptr->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + // If the map does not keep up with the current position, // lock ndt_ptr_ entirely until it is fully rebuilt. need_rebuild_ = true; @@ -65,8 +109,11 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi return distance > param_.update_distance; } -void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) +void MapUpdateModule::update_map( + const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) { + diagnostics_ptr->addKeyValue("is_need_rebuild", need_rebuild_); + // If the current position is super far from the previous loading position, // lock and rebuild ndt_ptr_ if (need_rebuild_) { @@ -82,25 +129,37 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) ndt_ptr_->setInputSource(input_source); } - const bool updated = update_ndt(position, *ndt_ptr_); + const bool updated = update_ndt(position, *ndt_ptr_, diagnostics_ptr); + + // check is_updated_map + diagnostics_ptr->addKeyValue("is_updated_map", updated); if (!updated) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 1000, - "update_ndt failed. If this happens with initial position estimation, make sure that" - "(1) the initial position matches the pcd map and (2) the map_loader is working properly."); + std::stringstream message; + message + << "update_ndt failed. If this happens with initial position estimation, make sure that" + << "(1) the initial position matches the pcd map and (2) the map_loader is working " + "properly."; + diagnostics_ptr->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str()); last_update_position_ = position; ndt_ptr_mutex_->unlock(); return; } + ndt_ptr_mutex_->unlock(); need_rebuild_ = false; + } else { // Load map to the secondary_ndt_ptr, which does not require a mutex lock // Since the update of the secondary ndt ptr and the NDT align (done on // the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly. // If the updating is done the main ndt_ptr_, either the update or the NDT // align will be blocked by the other. - const bool updated = update_ndt(position, *secondary_ndt_ptr_); + const bool updated = update_ndt(position, *secondary_ndt_ptr_, diagnostics_ptr); + + // check is_updated_map + diagnostics_ptr->addKeyValue("is_updated_map", updated); if (!updated) { last_update_position_ = position; return; @@ -128,8 +187,12 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) publish_partial_pcd_map(); } -bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt) +bool MapUpdateModule::update_ndt( + const geometry_msgs::msg::Point & position, NdtType & ndt, + std::unique_ptr & diagnostics_ptr) { + diagnostics_ptr->addKeyValue("maps_size_before", ndt.getCurrentMapIDs().size()); + auto request = std::make_shared(); request->area.center_x = static_cast(position.x); @@ -138,7 +201,13 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt request->cached_ids = ndt.getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); + diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); + + std::stringstream message; + message << "Waiting for pcd loader service. Check the pointcloud_map_loader."; + diagnostics_ptr->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return false; } // send a request to map_loader @@ -148,20 +217,27 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt std::future_status status = result.wait_for(std::chrono::seconds(0)); while (status != std::future_status::ready) { - RCLCPP_INFO(logger_, "waiting response"); + // check is_succeed_call_pcd_loader if (!rclcpp::ok()) { + diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); + + std::stringstream message; + message << "pcd_loader service is not working."; + diagnostics_ptr->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; // No update } status = result.wait_for(std::chrono::seconds(1)); } + diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true); auto & maps_to_add = result.get()->new_pointcloud_with_ids; auto & map_ids_to_remove = result.get()->ids_to_remove; - RCLCPP_INFO( - logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); + diagnostics_ptr->addKeyValue("maps_to_add_size", maps_to_add.size()); + diagnostics_ptr->addKeyValue("maps_to_remove_size", map_ids_to_remove.size()); + if (maps_to_add.empty() && map_ids_to_remove.empty()) { - RCLCPP_INFO(logger_, "Skip map update"); return false; // No update } @@ -187,7 +263,9 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; - RCLCPP_DEBUG(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); + diagnostics_ptr->addKeyValue("map_update_execution_time", exe_time); + diagnostics_ptr->addKeyValue("maps_size_after", ndt.getCurrentMapIDs().size()); + diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true); return true; // Updated } 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 ba4190d9f66fb..1e3188a15acc9 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -70,12 +70,9 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) tf2_buffer_(this->get_clock()), tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), - state_ptr_(new std::map), is_activated_(false), param_(this) { - (*state_ptr_)["state"] = "Initializing"; - timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -117,6 +114,9 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) const double value_as_unlimited = 1000.0; regularization_pose_buffer_ = std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); + + diagnostics_regularization_pose_ = + std::make_unique(this, "regularization_pose_subscriber_status"); } sensor_aligned_pose_pub_ = @@ -158,8 +158,6 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) this->create_publisher( "initial_to_result_distance_new", 10); ndt_marker_pub_ = this->create_publisher("ndt_marker", 10); - diagnostics_pub_ = - this->create_publisher("/diagnostics", 10); ndt_monte_carlo_initial_pose_marker_pub_ = this->create_publisher( "monte_carlo_initial_pose_marker", 10); @@ -184,112 +182,68 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); + diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); + diagnostics_initial_pose_ = + std::make_unique(this, "initial_pose_subscriber_status"); + diagnostics_map_update_ = std::make_unique(this, "map_update_status"); + diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); + diagnostics_trigger_node_ = + std::make_unique(this, "trigger_node_service_status"); + logger_configure_ = std::make_unique(this); } -void NDTScanMatcher::publish_diagnostic() +void NDTScanMatcher::callback_timer() { - diagnostic_msgs::msg::DiagnosticStatus diag_status_msg; - diag_status_msg.name = "ndt_scan_matcher"; - diag_status_msg.hardware_id = ""; - - for (const auto & key_value : (*state_ptr_)) { - diagnostic_msgs::msg::KeyValue key_value_msg; - key_value_msg.key = key_value.first; - key_value_msg.value = key_value.second; - diag_status_msg.values.push_back(key_value_msg); - } - - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - diag_status_msg.message = ""; - if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_status_msg.message += "Initializing State. "; - } - if ( - state_ptr_->count("lidar_topic_delay_time_sec") && - std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > - param_.validation.lidar_topic_timeout_sec) { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; - } - if ( - state_ptr_->count("skipping_publish_num") && - std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 && - std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_status_msg.message += "skipping_publish_num > 1. "; - } - if ( - state_ptr_->count("skipping_publish_num") && - std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - diag_status_msg.message += "skipping_publish_num exceed limit. "; - } - if ( - state_ptr_->count("nearest_voxel_transformation_likelihood") && - std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) < - param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood) { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_status_msg.message += "NDT score is unreliably low. "; - } - if ( - state_ptr_->count("execution_time") && std::stod((*state_ptr_)["execution_time"]) >= - param_.validation.critical_upper_bound_exe_time_ms) { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_status_msg.message += - "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; - } - // Ignore local optimal solution - if ( - state_ptr_->count("is_local_optimal_solution_oscillation") && - std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) { - diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - diag_status_msg.message = "local optimal solution oscillation occurred"; - } - - diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = this->now(); - diag_msg.status.push_back(diag_status_msg); - - diagnostics_pub_->publish(diag_msg); + diagnostics_map_update_->clear(); + + map_update_module_->callback_timer(is_activated_, latest_ekf_position_, diagnostics_map_update_); + + diagnostics_map_update_->publish(); } -void NDTScanMatcher::callback_timer() +void NDTScanMatcher::callback_initial_pose( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { - if (!is_activated_) { - return; - } - std::lock_guard lock(latest_ekf_position_mtx_); - if (latest_ekf_position_ == std::nullopt) { - RCLCPP_ERROR_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, - "Cannot find the reference position for map update. Please check if the EKF odometry is " - "provided to NDT."); - return; - } - // continue only if we should update the map - if (map_update_module_->should_update_map(latest_ekf_position_.value())) { - RCLCPP_INFO(this->get_logger(), "Start updating NDT map (timer_callback)"); - map_update_module_->update_map(latest_ekf_position_.value()); - } + diagnostics_initial_pose_->clear(); + + callback_initial_pose_main(initial_pose_msg_ptr); + + diagnostics_initial_pose_->publish(); } -void NDTScanMatcher::callback_initial_pose( +void NDTScanMatcher::callback_initial_pose_main( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { - if (!is_activated_) return; + diagnostics_initial_pose_->addKeyValue( + "topic_time_stamp", static_cast(initial_pose_msg_ptr->header.stamp).seconds()); - if (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame) { - initial_pose_buffer_->push_back(initial_pose_msg_ptr); - } else { - RCLCPP_ERROR_STREAM_THROTTLE( - get_logger(), *this->get_clock(), 1000, - "Received initial pose message with frame_id " - << initial_pose_msg_ptr->header.frame_id << ", but expected " << param_.frame.map_frame - << ". Please check the frame_id in the input topic and ensure it is correct."); + // check is_activated + diagnostics_initial_pose_->addKeyValue("is_activated", static_cast(is_activated_)); + if (!is_activated_) { + std::stringstream message; + message << "Node is not activated."; + diagnostics_initial_pose_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return; } + // check is_expected_frame_id + const bool is_expected_frame_id = + (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame); + diagnostics_initial_pose_->addKeyValue("is_expected_frame_id", is_expected_frame_id); + if (!is_expected_frame_id) { + std::stringstream message; + message << "Received initial pose message with frame_id " + << initial_pose_msg_ptr->header.frame_id << ", but expected " << param_.frame.map_frame + << ". Please check the frame_id in the input topic and ensure it is correct."; + diagnostics_initial_pose_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + return; + } + + initial_pose_buffer_->push_back(initial_pose_msg_ptr); + { // latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock std::lock_guard lock(latest_ekf_position_mtx_); @@ -300,41 +254,82 @@ void NDTScanMatcher::callback_initial_pose( void NDTScanMatcher::callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr) { + diagnostics_regularization_pose_->clear(); + + diagnostics_regularization_pose_->addKeyValue( + "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).seconds()); + regularization_pose_buffer_->push_back(pose_conv_msg_ptr); + + diagnostics_regularization_pose_->publish(); } void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame) { - if (sensor_points_msg_in_sensor_frame->data.empty()) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "Empty sensor points!"); - return; + // clear diagnostics + diagnostics_scan_points_->clear(); + + // scan matching + const bool is_succeed_scan_matching = + callback_sensor_points_main(sensor_points_msg_in_sensor_frame); + + // check skipping_publish_num + static size_t skipping_publish_num = 0; + const size_t error_skipping_publish_num = 5; + skipping_publish_num = is_succeed_scan_matching ? 0 : (skipping_publish_num + 1); + diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num); + if (skipping_publish_num >= error_skipping_publish_num) { + std::stringstream message; + message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times)."; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); } + diagnostics_scan_points_->publish(); +} + +bool NDTScanMatcher::callback_sensor_points_main( + sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame) +{ + const auto exe_start_time = std::chrono::system_clock::now(); + + // check topic_time_stamp const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; - const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); - (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); + diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.seconds()); + + // check sensor_points_size + const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width; + diagnostics_scan_points_->addKeyValue("sensor_points_size", sensor_points_size); + if (sensor_points_size == 0) { + std::stringstream message; + message << "Sensor points is empty."; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return false; + } - if (lidar_topic_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { - RCLCPP_WARN( - this->get_logger(), - "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " - "%lf[sec])", - lidar_topic_delay_time_sec, param_.validation.lidar_topic_timeout_sec); + // check sensor_points_delay_time_sec + const double sensor_points_delay_time_sec = + (this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds(); + diagnostics_scan_points_->addKeyValue( + "sensor_points_delay_time_sec", sensor_points_delay_time_sec); + if (sensor_points_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { + std::stringstream message; + message << "sensor points is experiencing latency." + << "The delay time is " << sensor_points_delay_time_sec << "[sec] " + << "(the tolerance is " << param_.validation.lidar_topic_timeout_sec << "[sec])."; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, // even if further processing continues, the estimated result will be rejected by ekf_localizer. // Therefore, it would be acceptable to exit the function here. // However, for now, we will continue the processing as it is. - // return; + // return false; } - // mutex ndt_ptr_ - std::lock_guard lock(ndt_ptr_mtx_); - - const auto exe_start_time = std::chrono::system_clock::now(); - // preprocess input pointcloud pcl::shared_ptr> sensor_points_in_sensor_frame( new pcl::PointCloud); @@ -343,34 +338,73 @@ void NDTScanMatcher::callback_sensor_points( const std::string & sensor_frame = sensor_points_msg_in_sensor_frame->header.frame_id; pcl::fromROSMsg(*sensor_points_msg_in_sensor_frame, *sensor_points_in_sensor_frame); - transform_sensor_measurement( - sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, - sensor_points_in_baselink_frame); - // check max distance of sensor points + // transform sensor points from sensor-frame to base_link + try { + transform_sensor_measurement( + sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, + sensor_points_in_baselink_frame); + } catch (const std::exception & ex) { + std::stringstream message; + message << ex.what() << ". Please publish TF " << sensor_frame << " to " + << param_.frame.base_frame; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", false); + return false; + } + diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", true); + + // check sensor_points_max_distance double max_distance = 0.0; for (const auto & point : sensor_points_in_baselink_frame->points) { const double distance = std::hypot(point.x, point.y, point.z); max_distance = std::max(max_distance, distance); } + + diagnostics_scan_points_->addKeyValue("sensor_points_max_distance", max_distance); if (max_distance < param_.sensor_points.required_distance) { - RCLCPP_WARN_STREAM( - this->get_logger(), "Max distance of sensor points = " - << std::fixed << std::setprecision(3) << max_distance << " [m] < " - << param_.sensor_points.required_distance << " [m]"); - return; + std::stringstream message; + message << "Max distance of sensor points = " << std::fixed << std::setprecision(3) + << max_distance << " [m] < " << param_.sensor_points.required_distance << " [m]"; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return false; } + // lock mutex + std::lock_guard lock(ndt_ptr_mtx_); + + // set sensor points to ndt class ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); - if (!is_activated_) return; + + // check is_activated + diagnostics_scan_points_->addKeyValue("is_activated", static_cast(is_activated_)); + if (!is_activated_) { + std::stringstream message; + message << "Node is not activated."; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return false; + } // calculate initial pose std::optional interpolation_result_opt = initial_pose_buffer_->interpolate(sensor_ros_time); - if (!interpolation_result_opt) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No interpolated pose!"); - return; + + // check is_succeed_interpolate_initial_pose + const bool is_succeed_interpolate_initial_pose = (interpolation_result_opt != std::nullopt); + diagnostics_scan_points_->addKeyValue( + "is_succeed_interpolate_initial_pose", is_succeed_interpolate_initial_pose); + if (!is_succeed_interpolate_initial_pose) { + std::stringstream message; + message << "Couldn't interpolate pose. Please check the initial pose topic"; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return false; } + initial_pose_buffer_->pop_old(sensor_ros_time); const SmartPoseBuffer::InterpolateResult & interpolation_result = interpolation_result_opt.value(); @@ -380,9 +414,15 @@ void NDTScanMatcher::callback_sensor_points( add_regularization_pose(sensor_ros_time); } - if (ndt_ptr_->getInputTarget() == nullptr) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No MAP!"); - return; + // check is_set_map_points + const bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); + diagnostics_scan_points_->addKeyValue("is_set_map_points", is_set_map_points); + if (!is_set_map_points) { + std::stringstream message; + message << "Map points is not set."; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + return false; } // perform ndt scan matching @@ -399,26 +439,68 @@ void NDTScanMatcher::callback_sensor_points( transformation_msg_array.push_back(pose_ros); } - // perform several validations - bool is_ok_iteration_num = - validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations()); - const int oscillation_num = count_oscillation(transformation_msg_array); - bool is_local_optimal_solution_oscillation = false; + // check iteration_num + diagnostics_scan_points_->addKeyValue("iteration_num", ndt_result.iteration_num); + const bool is_ok_iteration_num = (ndt_result.iteration_num < ndt_ptr_->getMaximumIterations()); if (!is_ok_iteration_num) { - constexpr int oscillation_threshold = 10; - is_local_optimal_solution_oscillation = (oscillation_num > oscillation_threshold); + std::stringstream message; + message << "The number of iterations has reached its upper limit. The number of iterations: " + << ndt_result.iteration_num << ", Limit: " << ndt_ptr_->getMaximumIterations() << "."; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } - bool is_ok_converged_param = validate_converged_param( - ndt_result.transform_probability, ndt_result.nearest_voxel_transformation_likelihood); - bool is_converged = is_ok_iteration_num && is_ok_converged_param; - static size_t skipping_publish_num = 0; - if (is_converged) { - skipping_publish_num = 0; + + // check local_optimal_solution_oscillation_num + constexpr int oscillation_num_threshold = 10; + const int oscillation_num = count_oscillation(transformation_msg_array); + diagnostics_scan_points_->addKeyValue("local_optimal_solution_oscillation_num", oscillation_num); + const bool is_local_optimal_solution_oscillation = (oscillation_num > oscillation_num_threshold); + if (is_local_optimal_solution_oscillation) { + std::stringstream message; + message << "There is a possibility of oscillation in a local minimum"; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + + // check score + diagnostics_scan_points_->addKeyValue("transform_probability", ndt_result.transform_probability); + diagnostics_scan_points_->addKeyValue( + "nearest_voxel_transformation_likelihood", ndt_result.nearest_voxel_transformation_likelihood); + std::string score_name = ""; + double score = 0.0; + double score_threshold = 0.0; + if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { + score_name = "Transform Probability"; + score = ndt_result.transform_probability; + score_threshold = param_.score_estimation.converged_param_transform_probability; + } else if ( + param_.score_estimation.converged_param_type == + ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + score_name = "Nearest Voxel Transformation Likelihood"; + score = ndt_result.nearest_voxel_transformation_likelihood; + score_threshold = + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood; } else { - ++skipping_publish_num; - RCLCPP_WARN(get_logger(), "Not Converged"); + std::stringstream message; + message << "Unknown converged param type. Please check `score_estimation.converged_param_type`"; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + return false; + } + + bool is_ok_score = (score > score_threshold); + if (!is_ok_score) { + std::stringstream message; + message << "Score is below the threshold. Score: " << score + << ", Threshold: " << score_threshold; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + RCLCPP_WARN_STREAM(this->get_logger(), message.str()); } + // check is_converged + bool is_converged = (is_ok_iteration_num || is_local_optimal_solution_oscillation) && is_ok_score; + // covariance estimation const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond( result_pose_msg.orientation.w, result_pose_msg.orientation.x, result_pose_msg.orientation.y, @@ -435,10 +517,31 @@ void NDTScanMatcher::callback_sensor_points( ndt_covariance = estimated_covariance; } + // check distance_initial_to_result + const auto distance_initial_to_result = static_cast( + norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); + diagnostics_scan_points_->addKeyValue("distance_initial_to_result", distance_initial_to_result); + const double warn_distance_initial_to_result = 3.0; + if (distance_initial_to_result > warn_distance_initial_to_result) { + std::stringstream message; + message << "distance_initial_to_result is too large (" << distance_initial_to_result + << " [m])."; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + + // check execution_time const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0f; + diagnostics_scan_points_->addKeyValue("execution_time", exe_time); + if (exe_time > param_.validation.critical_upper_bound_exe_time_ms) { + std::stringstream message; + message << "NDT exe time is too long (took " << exe_time << " [ms])."; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } // publish initial_pose_with_covariance_pub_->publish(interpolation_result.interpolated_pose); @@ -492,18 +595,7 @@ void NDTScanMatcher::callback_sensor_points( make_float32_stamped(sensor_ros_time, no_ground_nearest_voxel_transformation_likelihood)); } - (*state_ptr_)["state"] = "Aligned"; - (*state_ptr_)["transform_probability"] = std::to_string(ndt_result.transform_probability); - (*state_ptr_)["nearest_voxel_transformation_likelihood"] = - std::to_string(ndt_result.nearest_voxel_transformation_likelihood); - (*state_ptr_)["iteration_num"] = std::to_string(ndt_result.iteration_num); - (*state_ptr_)["skipping_publish_num"] = std::to_string(skipping_publish_num); - (*state_ptr_)["oscillation_count"] = std::to_string(oscillation_num); - (*state_ptr_)["is_local_optimal_solution_oscillation"] = - std::to_string(is_local_optimal_solution_oscillation); - (*state_ptr_)["execution_time"] = std::to_string(exe_time); - - publish_diagnostic(); + return is_converged; } void NDTScanMatcher::transform_sensor_measurement( @@ -520,12 +612,7 @@ void NDTScanMatcher::transform_sensor_measurement( try { transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - RCLCPP_WARN( - this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - // Since there is no clear error handling policy, temporarily return as is. - sensor_points_output_ptr = sensor_points_input_ptr; - return; + throw; } const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = @@ -638,54 +725,6 @@ void NDTScanMatcher::publish_initial_to_result( make_float32_stamped(sensor_ros_time, initial_to_result_distance_new)); } -bool NDTScanMatcher::validate_num_iteration(const int iter_num, const int max_iter_num) -{ - bool is_ok_iter_num = iter_num < max_iter_num; - if (!is_ok_iter_num) { - RCLCPP_WARN( - get_logger(), - "The number of iterations has reached its upper limit. The number of iterations: %d, Limit: " - "%d", - iter_num, max_iter_num); - } - return is_ok_iter_num; -} - -bool NDTScanMatcher::validate_score( - const double score, const double score_threshold, const std::string & score_name) -{ - bool is_ok_score = score > score_threshold; - if (!is_ok_score) { - RCLCPP_WARN( - get_logger(), "%s is below the threshold. Score: %lf, Threshold: %lf", score_name.c_str(), - score, score_threshold); - } - return is_ok_score; -} - -bool NDTScanMatcher::validate_converged_param( - const double & transform_probability, const double & nearest_voxel_transformation_likelihood) -{ - bool is_ok_converged_param = false; - if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { - is_ok_converged_param = validate_score( - transform_probability, param_.score_estimation.converged_param_transform_probability, - "Transform Probability"); - } else if ( - param_.score_estimation.converged_param_type == - ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { - is_ok_converged_param = validate_score( - nearest_voxel_transformation_likelihood, - param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood, - "Nearest Voxel Transformation Likelihood"); - } else { - is_ok_converged_param = false; - RCLCPP_ERROR_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1, "Unknown converged param type."); - } - return is_ok_converged_param; -} - int NDTScanMatcher::count_oscillation( const std::vector & result_pose_msg_array) { @@ -744,7 +783,10 @@ std::array NDTScanMatcher::estimate_covariance( rot = find_rotation_matrix_aligning_covariance_to_principal_axes( ndt_result.hessian.inverse().block(0, 0, 2, 2)); } catch (const std::exception & e) { - RCLCPP_WARN(get_logger(), "Error in Eigen solver: %s", e.what()); + std::stringstream message; + message << "Error in Eigen solver: " << e.what(); + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + return param_.covariance.output_pose_covariance; } @@ -822,24 +864,53 @@ void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_tim interpolation_result_opt.value(); const Eigen::Matrix4f pose = pose_to_matrix4f(interpolation_result.interpolated_pose.pose.pose); ndt_ptr_->setRegularizationPose(pose); - RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); } void NDTScanMatcher::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { + diagnostics_trigger_node_->clear(); + diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", this->now().seconds()); + is_activated_ = req->data; if (is_activated_) { initial_pose_buffer_->clear(); } res->success = true; + + diagnostics_trigger_node_->addKeyValue("is_activated", static_cast(is_activated_)); + diagnostics_trigger_node_->addKeyValue("is_succeed_service", res->success); + diagnostics_trigger_node_->publish(); } void NDTScanMatcher::service_ndt_align( const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { + diagnostics_ndt_align_->clear(); + + service_ndt_align_main(req, res); + + // check is_succeed_service + bool is_succeed_service = res->success; + diagnostics_ndt_align_->addKeyValue("is_succeed_service", is_succeed_service); + if (!is_succeed_service) { + std::stringstream message; + message << "ndt_align_service is failed."; + diagnostics_ndt_align_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + + diagnostics_ndt_align_->publish(); +} + +void NDTScanMatcher::service_ndt_align_main( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) +{ + diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", this->now().seconds()); + // get TF from pose_frame to map_frame const std::string & target_frame = param_.frame.map_frame; const std::string & source_frame = req->pose_with_covariance.header.frame_id; @@ -851,32 +922,50 @@ void NDTScanMatcher::service_ndt_align( // Note: Up to AWSIMv1.1.0, there is a known bug where the GNSS frame_id is incorrectly set to // "gnss_link" instead of "map". The ndt_align is designed to return identity when this issue // occurs. However, in the future, converting to a non-existent frame_id should be prohibited. - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - RCLCPP_WARN( - this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - transform_s2t.header.stamp = get_clock()->now(); - transform_s2t.header.frame_id = target_frame; - transform_s2t.child_frame_id = source_frame; - transform_s2t.transform = tf2::toMsg(tf2::Transform::getIdentity()); + + diagnostics_ndt_align_->addKeyValue("is_succeed_transform_initial_pose", false); + + std::stringstream message; + message << "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str(); + diagnostics_ndt_align_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + res->success = false; + return; } + diagnostics_ndt_align_->addKeyValue("is_succeed_transform_initial_pose", true); // transform pose_frame to map_frame const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); - map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + map_update_module_->update_map( + initial_pose_msg_in_map_frame.pose.pose.position, diagnostics_ndt_align_); // mutex Map std::lock_guard lock(ndt_ptr_mtx_); - if (ndt_ptr_->getInputTarget() == nullptr) { + // check is_set_map_points + bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); + diagnostics_ndt_align_->addKeyValue("is_set_map_points", is_set_map_points); + if (!is_set_map_points) { + std::stringstream message; + message << "No InputTarget. Please check the map file and the map_loader service"; + diagnostics_ndt_align_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); res->success = false; - RCLCPP_WARN( - get_logger(), "No InputTarget. Please check the map file and the map_loader service"); return; } - if (ndt_ptr_->getInputSource() == nullptr) { + // check is_set_sensor_points + bool is_set_sensor_points = (ndt_ptr_->getInputSource() != nullptr); + diagnostics_ndt_align_->addKeyValue("is_set_sensor_points", is_set_sensor_points); + if (!is_set_sensor_points) { + std::stringstream message; + message << "No InputSource. Please check the input lidar topic"; + diagnostics_ndt_align_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); res->success = false; - RCLCPP_WARN(get_logger(), "No InputSource. Please check the input lidar topic"); return; } @@ -1007,7 +1096,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); - RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score); + diagnostics_ndt_align_->addKeyValue("best_particle_score", best_particle_ptr->score); return result_pose_with_cov_msg; }