Skip to content

Commit

Permalink
feat: tier4_debug_msgs changed to autoware-internal_debug_msgs in fil…
Browse files Browse the repository at this point in the history 
…es localization/autoware_ndt_scan_matcher (#9861)

Signed-off-by: vish0012 <[email protected]>
Co-authored-by: SakodaShintaro <[email protected]>
  • Loading branch information
@vish0012 @SakodaShintaro
vish0012 and SakodaShintaro authored Jan 9, 2025
1 parent 4215995 commit 7ac40b7
Show file tree
Hide file tree
Showing 4 changed files with 51 additions and 47 deletions.
40 changes: 20 additions & 20 deletions localization/autoware_ndt_scan_matcher/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
23 changes: 12 additions & 11 deletions ...ion/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,15 +26,15 @@
#include <autoware/universe_utils/ros/logger_level_configure.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_internal_debug_msgs/msg/float32_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/int32_stamped.hpp>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_srvs/srv/set_bool.hpp>
#include <tier4_debug_msgs/msg/float32_stamped.hpp>
#include <tier4_debug_msgs/msg/int32_stamped.hpp>
#include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

Expand Down Expand Up @@ -166,23 +166,24 @@ class NDTScanMatcher : public rclcpp::Node
initial_pose_with_covariance_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr multi_ndt_pose_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr multi_initial_pose_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr exe_time_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr transform_probability_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr exe_time_pub_;
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
transform_probability_pub_;
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
nearest_voxel_transformation_likelihood_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr voxel_score_points_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
no_ground_transform_probability_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
no_ground_nearest_voxel_transformation_likelihood_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Int32Stamped>::SharedPtr iteration_num_pub_;
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Int32Stamped>::SharedPtr iteration_num_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
initial_to_result_relative_pose_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
initial_to_result_distance_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
initial_to_result_distance_old_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float32Stamped>::SharedPtr
initial_to_result_distance_new_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr ndt_marker_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
Expand Down
2 changes: 1 addition & 1 deletion localization/autoware_ndt_scan_matcher/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_internal_debug_msgs</depend>
<depend>autoware_localization_util</depend>
<depend>autoware_map_msgs</depend>
<depend>autoware_universe_utils</depend>
Expand All @@ -35,7 +36,6 @@
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tf2_sensor_msgs</depend>
<depend>tier4_debug_msgs</depend>
<depend>tier4_localization_msgs</depend>
<depend>visualization_msgs</depend>

Expand Down
33 changes: 18 additions & 15 deletions localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<T>().stamp(stamp).data(data);
using T = autoware_internal_debug_msgs::msg::Float32Stamped;
return autoware_internal_debug_msgs::build<T>().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<T>().stamp(stamp).data(data);
using T = autoware_internal_debug_msgs::msg::Int32Stamped;
return autoware_internal_debug_msgs::build<T>().stamp(stamp).data(data);
}

std::array<double, 36> rotate_covariance(
Expand Down Expand Up @@ -158,31 +158,34 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options)
multi_ndt_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("multi_ndt_pose", 10);
multi_initial_pose_pub_ =
this->create_publisher<geometry_msgs::msg::PoseArray>("multi_initial_pose", 10);
exe_time_pub_ = this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("exe_time_ms", 10);
exe_time_pub_ =
this->create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>("exe_time_ms", 10);
transform_probability_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("transform_probability", 10);
this->create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>(
"transform_probability", 10);
nearest_voxel_transformation_likelihood_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
this->create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>(
"nearest_voxel_transformation_likelihood", 10);
voxel_score_points_pub_ =
this->create_publisher<sensor_msgs::msg::PointCloud2>("voxel_score_points", 10);
no_ground_transform_probability_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
this->create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>(
"no_ground_transform_probability", 10);
no_ground_nearest_voxel_transformation_likelihood_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
this->create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>(
"no_ground_nearest_voxel_transformation_likelihood", 10);
iteration_num_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Int32Stamped>("iteration_num", 10);
this->create_publisher<autoware_internal_debug_msgs::msg::Int32Stamped>("iteration_num", 10);
initial_to_result_relative_pose_pub_ =
this->create_publisher<geometry_msgs::msg::PoseStamped>("initial_to_result_relative_pose", 10);
initial_to_result_distance_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("initial_to_result_distance", 10);
this->create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>(
"initial_to_result_distance", 10);
initial_to_result_distance_old_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
this->create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>(
"initial_to_result_distance_old", 10);
initial_to_result_distance_new_pub_ =
this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
this->create_publisher<autoware_internal_debug_msgs::msg::Float32Stamped>(
"initial_to_result_distance_new", 10);
ndt_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("ndt_marker", 10);
ndt_monte_carlo_initial_pose_marker_pub_ =
Expand Down

0 comments on commit 7ac40b7

Please sign in to comment.