From 7ac40b7d5e29bee9a1eeed604e9340fdbd58e762 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Thu, 9 Jan 2025 15:30:58 +0900 Subject: [PATCH] feat: tier4_debug_msgs changed to autoware-internal_debug_msgs in files localization/autoware_ndt_scan_matcher (#9861) Signed-off-by: vish0012 Co-authored-by: SakodaShintaro --- .../autoware_ndt_scan_matcher/README.md | 40 +++++++++---------- .../ndt_scan_matcher_core.hpp | 23 ++++++----- .../autoware_ndt_scan_matcher/package.xml | 2 +- .../src/ndt_scan_matcher_core.cpp | 33 ++++++++------- 4 files changed, 51 insertions(+), 47 deletions(-) diff --git a/localization/autoware_ndt_scan_matcher/README.md b/localization/autoware_ndt_scan_matcher/README.md index 22e56930a0048..ee177608bdefe 100644 --- a/localization/autoware_ndt_scan_matcher/README.md +++ b/localization/autoware_ndt_scan_matcher/README.md @@ -25,26 +25,26 @@ One optional function is regularization. Please see the regularization chapter i ### Output -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | -| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | -| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | -| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | -| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | -| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | -| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | -| `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | -| `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | -| `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | -| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | -| `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | -| `initial_to_result_distance_old` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | -| `initial_to_result_distance_new` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | -| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | -| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | +| Name | Type | Description | +| --------------------------------- | --------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | +| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | +| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | +| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | +| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | +| `exe_time_ms` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | +| `transform_probability` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | +| `no_ground_transform_probability` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | +| `iteration_num` | `autoware_internal_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | +| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | +| `initial_to_result_distance` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | +| `initial_to_result_distance_old` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | +| `initial_to_result_distance_new` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | +| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | +| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | ### Service diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index e083e6751c3db..d6e629ee2f1c3 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -26,6 +26,8 @@ #include #include +#include +#include #include #include #include @@ -33,8 +35,6 @@ #include #include #include -#include -#include #include #include @@ -166,23 +166,24 @@ class NDTScanMatcher : public rclcpp::Node initial_pose_with_covariance_pub_; rclcpp::Publisher::SharedPtr multi_ndt_pose_pub_; rclcpp::Publisher::SharedPtr multi_initial_pose_pub_; - rclcpp::Publisher::SharedPtr exe_time_pub_; - rclcpp::Publisher::SharedPtr transform_probability_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr exe_time_pub_; + rclcpp::Publisher::SharedPtr + transform_probability_pub_; + rclcpp::Publisher::SharedPtr nearest_voxel_transformation_likelihood_pub_; rclcpp::Publisher::SharedPtr voxel_score_points_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr no_ground_transform_probability_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr no_ground_nearest_voxel_transformation_likelihood_pub_; - rclcpp::Publisher::SharedPtr iteration_num_pub_; + rclcpp::Publisher::SharedPtr iteration_num_pub_; rclcpp::Publisher::SharedPtr initial_to_result_relative_pose_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_old_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_new_pub_; rclcpp::Publisher::SharedPtr ndt_marker_pub_; rclcpp::Publisher::SharedPtr diff --git a/localization/autoware_ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml index f2047418310aa..ebd1ebda2d573 100644 --- a/localization/autoware_ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_localization_util autoware_map_msgs autoware_universe_utils @@ -35,7 +36,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_debug_msgs tier4_localization_msgs visualization_msgs diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index d3e1fa5982867..3d06dfffa16ed 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -53,18 +53,18 @@ using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; using autoware::universe_utils::DiagnosticsInterface; -tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( +autoware_internal_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) { - using T = tier4_debug_msgs::msg::Float32Stamped; - return tier4_debug_msgs::build().stamp(stamp).data(data); + using T = autoware_internal_debug_msgs::msg::Float32Stamped; + return autoware_internal_debug_msgs::build().stamp(stamp).data(data); } -tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( +autoware_internal_debug_msgs::msg::Int32Stamped make_int32_stamped( const builtin_interfaces::msg::Time & stamp, const int32_t data) { - using T = tier4_debug_msgs::msg::Int32Stamped; - return tier4_debug_msgs::build().stamp(stamp).data(data); + using T = autoware_internal_debug_msgs::msg::Int32Stamped; + return autoware_internal_debug_msgs::build().stamp(stamp).data(data); } std::array rotate_covariance( @@ -158,31 +158,34 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) multi_ndt_pose_pub_ = this->create_publisher("multi_ndt_pose", 10); multi_initial_pose_pub_ = this->create_publisher("multi_initial_pose", 10); - exe_time_pub_ = this->create_publisher("exe_time_ms", 10); + exe_time_pub_ = + this->create_publisher("exe_time_ms", 10); transform_probability_pub_ = - this->create_publisher("transform_probability", 10); + this->create_publisher( + "transform_probability", 10); nearest_voxel_transformation_likelihood_pub_ = - this->create_publisher( + this->create_publisher( "nearest_voxel_transformation_likelihood", 10); voxel_score_points_pub_ = this->create_publisher("voxel_score_points", 10); no_ground_transform_probability_pub_ = - this->create_publisher( + this->create_publisher( "no_ground_transform_probability", 10); no_ground_nearest_voxel_transformation_likelihood_pub_ = - this->create_publisher( + this->create_publisher( "no_ground_nearest_voxel_transformation_likelihood", 10); iteration_num_pub_ = - this->create_publisher("iteration_num", 10); + this->create_publisher("iteration_num", 10); initial_to_result_relative_pose_pub_ = this->create_publisher("initial_to_result_relative_pose", 10); initial_to_result_distance_pub_ = - this->create_publisher("initial_to_result_distance", 10); + this->create_publisher( + "initial_to_result_distance", 10); initial_to_result_distance_old_pub_ = - this->create_publisher( + this->create_publisher( "initial_to_result_distance_old", 10); initial_to_result_distance_new_pub_ = - this->create_publisher( + this->create_publisher( "initial_to_result_distance_new", 10); ndt_marker_pub_ = this->create_publisher("ndt_marker", 10); ndt_monte_carlo_initial_pose_marker_pub_ =