From e8182ff623de64482e30d72db1a169dd8963eae9 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 10 Jun 2024 16:03:09 +0900 Subject: [PATCH] Revert "refactor(ndt scan matcher): apply static analysis (#7278)" This reverts commit 3b90dc0b6159354d2529139fb4b09d3f24befeda. --- .../ndt_scan_matcher/diagnostics_module.hpp | 16 +- .../ndt_scan_matcher/hyper_parameters.hpp | 64 ++++---- .../ndt_scan_matcher_core.hpp | 2 + .../src/diagnostics_module.cpp | 16 +- .../src/map_update_module.cpp | 42 ++--- .../src/ndt_scan_matcher_core.cpp | 150 +++++++++--------- .../ndt_scan_matcher/test/test_util.hpp | 5 - 7 files changed, 145 insertions(+), 150 deletions(-) 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 index 6dfea386abaf8..4f7b5eff6521b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp @@ -27,14 +27,14 @@ class DiagnosticsModule public: DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); - void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); + void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); template - void add_key_value(const std::string & key, const T & value); - void update_level_and_message(const int8_t level, const std::string & message); + void addKeyValue(const std::string & key, const T & value); + void updateLevelAndMessage(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); private: - [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; @@ -44,17 +44,17 @@ class DiagnosticsModule }; template -void DiagnosticsModule::add_key_value(const std::string & key, const T & value) +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); - add_key_value(key_value); + addKeyValue(key_value); } template <> -void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); template <> -void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); +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 7002b4bf43a73..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 @@ -33,62 +33,62 @@ struct HyperParameters { struct Frame { - std::string base_frame{}; - std::string ndt_base_frame{}; - std::string map_frame{}; - } frame{}; + std::string base_frame; + std::string ndt_base_frame; + std::string map_frame; + } frame; struct SensorPoints { - double required_distance{}; - } sensor_points{}; + double required_distance; + } sensor_points; - pclomp::NdtParams ndt{}; - bool ndt_regularization_enable{}; + pclomp::NdtParams ndt; + bool ndt_regularization_enable; struct InitialPoseEstimation { - int64_t particles_num{}; - int64_t n_startup_trials{}; - } initial_pose_estimation{}; + int64_t particles_num; + int64_t n_startup_trials; + } initial_pose_estimation; struct Validation { - double lidar_topic_timeout_sec{}; - double initial_pose_timeout_sec{}; - double initial_pose_distance_tolerance_m{}; - double critical_upper_bound_exe_time_ms{}; - } validation{}; + double lidar_topic_timeout_sec; + double initial_pose_timeout_sec; + double initial_pose_distance_tolerance_m; + double critical_upper_bound_exe_time_ms; + } validation; struct ScoreEstimation { - ConvergedParamType converged_param_type{}; - double converged_param_transform_probability{}; - double converged_param_nearest_voxel_transformation_likelihood{}; + ConvergedParamType converged_param_type; + double converged_param_transform_probability; + double converged_param_nearest_voxel_transformation_likelihood; struct NoGroundPoints { - bool enable{}; - double z_margin_for_ground_removal{}; - } no_ground_points{}; - } score_estimation{}; + bool enable; + double z_margin_for_ground_removal; + } no_ground_points; + } score_estimation; struct Covariance { - std::array output_pose_covariance{}; + std::array output_pose_covariance; struct CovarianceEstimation { - bool enable{}; - std::vector initial_pose_offset_model{}; - } covariance_estimation{}; - } covariance{}; + bool enable; + std::vector initial_pose_offset_model; + } covariance_estimation; + } covariance; struct DynamicMapLoading { - double update_distance{}; - double map_radius{}; - double lidar_radius{}; - } dynamic_map_loading{}; + double update_distance; + double map_radius; + double lidar_radius; + } dynamic_map_loading; public: explicit HyperParameters(rclcpp::Node * node) 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 cbd2797c57922..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 @@ -131,6 +131,8 @@ class NDTScanMatcher : public rclcpp::Node static int count_oscillation(const std::vector & result_pose_msg_array); + std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const; std::array estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); diff --git a/localization/ndt_scan_matcher/src/diagnostics_module.cpp b/localization/ndt_scan_matcher/src/diagnostics_module.cpp index 805ee676c5e04..1e08ebb89f51e 100644 --- a/localization/ndt_scan_matcher/src/diagnostics_module.cpp +++ b/localization/ndt_scan_matcher/src/diagnostics_module.cpp @@ -41,7 +41,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +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), @@ -55,24 +55,24 @@ void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key } template <> -void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) +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; - add_key_value(key_value); + addKeyValue(key_value); } template <> -void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) +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"; - add_key_value(key_value); + addKeyValue(key_value); } -void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) +void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -87,10 +87,10 @@ void DiagnosticsModule::update_level_and_message(const int8_t level, const std:: void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); + diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray 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 32e5a0f2a7c3c..40f0b1f4465fa 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -53,23 +53,23 @@ void MapUpdateModule::callback_timer( std::unique_ptr & diagnostics_ptr) { // check is_activated - diagnostics_ptr->add_key_value("is_activated", is_activated); + diagnostics_ptr->addKeyValue("is_activated", is_activated); if (!is_activated) { std::stringstream message; message << "Node is not activated."; - diagnostics_ptr->update_level_and_message( + 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->add_key_value("is_set_last_update_position", is_set_last_update_position); + 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->update_level_and_message( + diagnostics_ptr->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -92,11 +92,11 @@ bool MapUpdateModule::should_update_map( const double distance = std::hypot(dx, dy); // check distance_last_update_position_to_current_position - diagnostics_ptr->add_key_value("distance_last_update_position_to_current_position", distance); + diagnostics_ptr->addKeyValue("distance_last_update_position_to_current_position", distance); if (distance + param_.lidar_radius > param_.map_radius) { std::stringstream message; message << "Dynamic map loading is not keeping up."; - diagnostics_ptr->update_level_and_message( + diagnostics_ptr->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); // If the map does not keep up with the current position, @@ -110,7 +110,7 @@ bool MapUpdateModule::should_update_map( void MapUpdateModule::update_map( const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); + 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_ @@ -130,14 +130,14 @@ void MapUpdateModule::update_map( const bool updated = update_ndt(position, *ndt_ptr_, diagnostics_ptr); // check is_updated_map - diagnostics_ptr->add_key_value("is_updated_map", updated); + diagnostics_ptr->addKeyValue("is_updated_map", updated); if (!updated) { 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->update_level_and_message( + diagnostics_ptr->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str()); last_update_position_ = position; @@ -157,7 +157,7 @@ void MapUpdateModule::update_map( const bool updated = update_ndt(position, *secondary_ndt_ptr_, diagnostics_ptr); // check is_updated_map - diagnostics_ptr->add_key_value("is_updated_map", updated); + diagnostics_ptr->addKeyValue("is_updated_map", updated); if (!updated) { last_update_position_ = position; return; @@ -189,7 +189,7 @@ bool MapUpdateModule::update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size()); + diagnostics_ptr->addKeyValue("maps_size_before", ndt.getCurrentMapIDs().size()); auto request = std::make_shared(); @@ -199,11 +199,11 @@ bool MapUpdateModule::update_ndt( request->cached_ids = ndt.getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false); + 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->update_level_and_message( + diagnostics_ptr->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -217,23 +217,23 @@ bool MapUpdateModule::update_ndt( while (status != std::future_status::ready) { // check is_succeed_call_pcd_loader if (!rclcpp::ok()) { - diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false); + diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); std::stringstream message; message << "pcd_loader service is not working."; - diagnostics_ptr->update_level_and_message( + diagnostics_ptr->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; // No update } status = result.wait_for(std::chrono::seconds(1)); } - diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true); + 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; - diagnostics_ptr->add_key_value("maps_to_add_size", maps_to_add.size()); - diagnostics_ptr->add_key_value("maps_to_remove_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()) { return false; // No update @@ -261,9 +261,9 @@ bool MapUpdateModule::update_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; - diagnostics_ptr->add_key_value("map_update_execution_time", exe_time); - diagnostics_ptr->add_key_value("maps_size_after", ndt.getCurrentMapIDs().size()); - diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true); + 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 c61e243caab31..349f924019c28 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -62,28 +62,6 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } -std::array rotate_covariance( - const std::array & src_covariance, const Eigen::Matrix3d & rotation) -{ - std::array ret_covariance = src_covariance; - - Eigen::Matrix3d src_cov; - src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], - src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], - src_covariance[14]; - - Eigen::Matrix3d ret_cov; - ret_cov = rotation * src_cov * rotation.transpose(); - - for (Eigen::Index i = 0; i < 3; ++i) { - ret_covariance[i] = ret_cov(0, i); - ret_covariance[i + 6] = ret_cov(1, i); - ret_covariance[i + 12] = ret_cov(2, i); - } - - return ret_covariance; -} - NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) : Node("ndt_scan_matcher", options), tf2_broadcaster_(*this), @@ -219,7 +197,7 @@ void NDTScanMatcher::callback_timer() diagnostics_map_update_->clear(); - diagnostics_map_update_->add_key_value("timer_callback_time_stamp", ros_time_now.nanoseconds()); + diagnostics_map_update_->addKeyValue("timer_callback_time_stamp", ros_time_now.nanoseconds()); map_update_module_->callback_timer(is_activated_, latest_ekf_position_, diagnostics_map_update_); @@ -239,16 +217,16 @@ void NDTScanMatcher::callback_initial_pose( void NDTScanMatcher::callback_initial_pose_main( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { - diagnostics_initial_pose_->add_key_value( + diagnostics_initial_pose_->addKeyValue( "topic_time_stamp", static_cast(initial_pose_msg_ptr->header.stamp).nanoseconds()); // check is_activated - diagnostics_initial_pose_->add_key_value("is_activated", static_cast(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_->update_level_and_message( + diagnostics_initial_pose_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -256,13 +234,13 @@ void NDTScanMatcher::callback_initial_pose_main( // 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_->add_key_value("is_expected_frame_id", is_expected_frame_id); + 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_->update_level_and_message( + diagnostics_initial_pose_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); return; } @@ -281,7 +259,7 @@ void NDTScanMatcher::callback_regularization_pose( { diagnostics_regularization_pose_->clear(); - diagnostics_regularization_pose_->add_key_value( + diagnostics_regularization_pose_->addKeyValue( "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).nanoseconds()); regularization_pose_buffer_->push_back(pose_conv_msg_ptr); @@ -304,11 +282,11 @@ void NDTScanMatcher::callback_sensor_points( const size_t error_skipping_publish_num = 5; skipping_publish_num = ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); - diagnostics_scan_points_->add_key_value("skipping_publish_num", skipping_publish_num); + 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -322,15 +300,15 @@ bool NDTScanMatcher::callback_sensor_points_main( // check topic_time_stamp const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; - diagnostics_scan_points_->add_key_value("topic_time_stamp", sensor_ros_time.nanoseconds()); + diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.nanoseconds()); // check sensor_points_size const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width; - diagnostics_scan_points_->add_key_value("sensor_points_size", sensor_points_size); + 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -338,14 +316,14 @@ bool NDTScanMatcher::callback_sensor_points_main( // 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_->add_key_value( + 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_->update_level_and_message( + 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, @@ -374,13 +352,13 @@ bool NDTScanMatcher::callback_sensor_points_main( std::stringstream message; message << ex.what() << ". Please publish TF " << sensor_frame << " to " << param_.frame.base_frame; - diagnostics_scan_points_->update_level_and_message( + 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_->add_key_value("is_succeed_transform_sensor_points", false); + diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", false); return false; } - diagnostics_scan_points_->add_key_value("is_succeed_transform_sensor_points", true); + diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", true); // check sensor_points_max_distance double max_distance = 0.0; @@ -389,12 +367,12 @@ bool NDTScanMatcher::callback_sensor_points_main( max_distance = std::max(max_distance, distance); } - diagnostics_scan_points_->add_key_value("sensor_points_max_distance", max_distance); + diagnostics_scan_points_->addKeyValue("sensor_points_max_distance", max_distance); if (max_distance < param_.sensor_points.required_distance) { 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -406,11 +384,11 @@ bool NDTScanMatcher::callback_sensor_points_main( ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); // check is_activated - diagnostics_scan_points_->add_key_value("is_activated", static_cast(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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -421,12 +399,12 @@ bool NDTScanMatcher::callback_sensor_points_main( // check is_succeed_interpolate_initial_pose const bool is_succeed_interpolate_initial_pose = (interpolation_result_opt != std::nullopt); - diagnostics_scan_points_->add_key_value( + 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -442,11 +420,11 @@ bool NDTScanMatcher::callback_sensor_points_main( // check is_set_map_points const bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); - diagnostics_scan_points_->add_key_value("is_set_map_points", is_set_map_points); + 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -466,33 +444,31 @@ bool NDTScanMatcher::callback_sensor_points_main( } // check iteration_num - diagnostics_scan_points_->add_key_value("iteration_num", ndt_result.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) { 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } // check local_optimal_solution_oscillation_num constexpr int oscillation_num_threshold = 10; const int oscillation_num = count_oscillation(transformation_msg_array); - diagnostics_scan_points_->add_key_value( - "local_optimal_solution_oscillation_num", oscillation_num); + 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } // check score - diagnostics_scan_points_->add_key_value( - "transform_probability", ndt_result.transform_probability); - diagnostics_scan_points_->add_key_value( + diagnostics_scan_points_->addKeyValue("transform_probability", ndt_result.transform_probability); + diagnostics_scan_points_->addKeyValue( "nearest_voxel_transformation_likelihood", ndt_result.nearest_voxel_transformation_likelihood); double score = 0.0; double score_threshold = 0.0; @@ -508,7 +484,7 @@ bool NDTScanMatcher::callback_sensor_points_main( } else { std::stringstream message; message << "Unknown converged param type. Please check `score_estimation.converged_param_type`"; - diagnostics_scan_points_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); return false; } @@ -550,7 +526,7 @@ bool NDTScanMatcher::callback_sensor_points_main( std::stringstream message; message << "Score is below the threshold. Score: " << score << ", Threshold: " << score_threshold; - diagnostics_scan_points_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); RCLCPP_WARN_STREAM(this->get_logger(), message.str()); } @@ -577,13 +553,13 @@ bool NDTScanMatcher::callback_sensor_points_main( // 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_->add_key_value("distance_initial_to_result", distance_initial_to_result); + 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -592,11 +568,11 @@ bool NDTScanMatcher::callback_sensor_points_main( 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_->add_key_value("execution_time", exe_time); + 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_->update_level_and_message( + diagnostics_scan_points_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -668,7 +644,7 @@ void NDTScanMatcher::transform_sensor_measurement( geometry_msgs::msg::TransformStamped transform; try { transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { + } catch (tf2::TransformException & ex) { throw; } @@ -809,6 +785,28 @@ int NDTScanMatcher::count_oscillation( return max_oscillation_cnt; } +std::array NDTScanMatcher::rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) @@ -908,7 +906,7 @@ void NDTScanMatcher::service_trigger_node( const rclcpp::Time ros_time_now = this->now(); diagnostics_trigger_node_->clear(); - diagnostics_trigger_node_->add_key_value("service_call_time_stamp", ros_time_now.nanoseconds()); + diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); is_activated_ = req->data; if (is_activated_) { @@ -916,8 +914,8 @@ void NDTScanMatcher::service_trigger_node( } res->success = true; - diagnostics_trigger_node_->add_key_value("is_activated", static_cast(is_activated_)); - diagnostics_trigger_node_->add_key_value("is_succeed_service", res->success); + diagnostics_trigger_node_->addKeyValue("is_activated", static_cast(is_activated_)); + diagnostics_trigger_node_->addKeyValue("is_succeed_service", res->success); diagnostics_trigger_node_->publish(ros_time_now); } @@ -929,17 +927,17 @@ void NDTScanMatcher::service_ndt_align( diagnostics_ndt_align_->clear(); - diagnostics_ndt_align_->add_key_value("service_call_time_stamp", ros_time_now.nanoseconds()); + diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); service_ndt_align_main(req, res); // check is_succeed_service bool is_succeed_service = res->success; - diagnostics_ndt_align_->add_key_value("is_succeed_service", is_succeed_service); + 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_->update_level_and_message( + diagnostics_ndt_align_->updateLevelAndMessage( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -962,17 +960,17 @@ void NDTScanMatcher::service_ndt_align_main( // "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. - diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", false); + 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_->update_level_and_message( + 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_->add_key_value("is_succeed_transform_initial_pose", true); + 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); @@ -984,11 +982,11 @@ void NDTScanMatcher::service_ndt_align_main( // check is_set_map_points bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); - diagnostics_ndt_align_->add_key_value("is_set_map_points", is_set_map_points); + 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_->update_level_and_message( + 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; @@ -997,11 +995,11 @@ void NDTScanMatcher::service_ndt_align_main( // check is_set_sensor_points bool is_set_sensor_points = (ndt_ptr_->getInputSource() != nullptr); - diagnostics_ndt_align_->add_key_value("is_set_sensor_points", is_set_sensor_points); + 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_->update_level_and_message( + 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; @@ -1109,7 +1107,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); - diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score); + diagnostics_ndt_align_->addKeyValue("best_particle_score", best_particle_ptr->score); return result_pose_with_cov_msg; } diff --git a/localization/ndt_scan_matcher/test/test_util.hpp b/localization/ndt_scan_matcher/test/test_util.hpp index dbf055d3c95d3..2f78827abf891 100644 --- a/localization/ndt_scan_matcher/test/test_util.hpp +++ b/localization/ndt_scan_matcher/test/test_util.hpp @@ -15,13 +15,8 @@ #ifndef TEST_UTIL_HPP_ #define TEST_UTIL_HPP_ -#include -#include - -#include #include #include -#include inline pcl::PointCloud make_sample_half_cubic_pcd() {