From a366c42e6b8108463665e29446ad22dfd96b6787 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 24 Jan 2024 13:08:34 +0100 Subject: [PATCH] build(yabloc_particle_filter): fix namespace for messages. Fix formatting Signed-off-by: Esteve Fernandez --- .../yabloc/yabloc_particle_filter/README.md | 82 +++++++++---------- .../src/common/mean.cpp | 12 +-- .../src/common/particle_visualize_node.cpp | 4 +- .../src/correction/correction_util.cpp | 4 +- .../src/prediction/predictor.cpp | 2 +- .../test/src/test_resampler.cpp | 4 +- 6 files changed, 54 insertions(+), 54 deletions(-) diff --git a/localization/yabloc/yabloc_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md index 41ab7b1b10039..a04ff94136bc4 100644 --- a/localization/yabloc/yabloc_particle_filter/README.md +++ b/localization/yabloc/yabloc_particle_filter/README.md @@ -17,22 +17,22 @@ This package contains some executable nodes related to particle filter. #### Input -| Name | Type | Description | -| ----------------------------- | ------------------------------------------------ | --------------------------------------------------------- | -| `input/initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | to specify the initial position of particles | -| `input/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | linear velocity and angular velocity of prediction update | -| `input/height` | `std_msgs::msg::Float32` | ground height | -| `input/weighted_particles` | `yabloc_particle_filter::msg::ParticleArray` | particles weighted by corrector nodes | +| Name | Type | Description | +| ----------------------------- | --------------------------------------------------------- | --------------------------------------------------------- | +| `input/initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | to specify the initial position of particles | +| `input/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | linear velocity and angular velocity of prediction update | +| `input/height` | `std_msgs::msg::Float32` | ground height | +| `input/weighted_particles` | `autoware_yabloc_particle_filter::msg::ParticleArray` | particles weighted by corrector nodes | #### Output -| Name | Type | Description | -| ------------------------------ | ----------------------------------------------- | --------------------------------------------------------- | -| `output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | particle centroid with covariance | -| `output/pose` | `geometry_msgs::msg::PoseStamped` | particle centroid with covariance | -| `output/predicted_particles` | `yabloc_particle_filter::msg::ParticleArray` | particles weighted by predictor nodes | -| `debug/init_marker` | `visualization_msgs::msg::Marker` | debug visualization of initial position | -| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | +| Name | Type | Description | +| ------------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | particle centroid with covariance | +| `output/pose` | `geometry_msgs::msg::PoseStamped` | particle centroid with covariance | +| `output/predicted_particles` | `autoware_yabloc_particle_filter::msg::ParticleArray` | particles weighted by predictor nodes | +| `debug/init_marker` | `visualization_msgs::msg::Marker` | debug visualization of initial position | +| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | ### Parameters @@ -57,20 +57,20 @@ This package contains some executable nodes related to particle filter. #### Input -| Name | Type | Description | -| ---------------------------- | ----------------------------------------------- | -------------------------------------------------- | -| `input/height` | `std_msgs::msg::Float32` | ground height | -| `input/predicted_particles` | `yabloc_particle_filter::msg::ParticleArray` | predicted particles | -| `input/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | gnss measurement. used if `use_ublox_msg` is false | -| `input/navpvt` | `ublox_msgs::msg::NavPVT` | gnss measurement. used if `use_ublox_msg` is true | +| Name | Type | Description | +| ---------------------------- | -------------------------------------------------------- | -------------------------------------------------- | +| `input/height` | `std_msgs::msg::Float32` | ground height | +| `input/predicted_particles` | `autoware_yabloc_particle_filter::msg::ParticleArray` | predicted particles | +| `input/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | gnss measurement. used if `use_ublox_msg` is false | +| `input/navpvt` | `ublox_msgs::msg::NavPVT` | gnss measurement. used if `use_ublox_msg` is true | #### Output -| Name | Type | Description | -| ------------------------------ | -------------------------------------------- | --------------------------------------------------------- | -| `output/weighted_particles` | `yabloc_particle_filter::msg::ParticleArray` | weighted particles | -| `debug/gnss_range_marker` | `visualization_msgs::msg::MarkerArray` | gnss weight distribution | -| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | +| Name | Type | Description | +| ------------------------------ | ----------------------------------------------------- | --------------------------------------------------------- | +| `output/weighted_particles` | `autoware_yabloc_particle_filter::msg::ParticleArray` | weighted particles | +| `debug/gnss_range_marker` | `visualization_msgs::msg::MarkerArray` | gnss weight distribution | +| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | ### Parameters @@ -98,26 +98,26 @@ This package contains some executable nodes related to particle filter. #### Input -| Name | Type | Description | -| ------------------------------------- | -------------------------------------------- | ----------------------------------------------------------- | -| `input/predicted_particles` | `yabloc_particle_filter::msg::ParticleArray` | predicted particles | -| `input/ll2_bounding_box` | `sensor_msgs::msg::PointCloud2` | road surface markings converted to line segments | -| `input/ll2_road_marking` | `sensor_msgs::msg::PointCloud2` | road surface markings converted to line segments | -| `input/projected_line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | projected line segments | -| `input/pose` | `geometry_msgs::msg::PoseStamped` | reference to retrieve the area map around the self location | +| Name | Type | Description | +| ------------------------------------- | ----------------------------------------------------- | ----------------------------------------------------------- | +| `input/predicted_particles` | `autoware_yabloc_particle_filter::msg::ParticleArray` | predicted particles | +| `input/ll2_bounding_box` | `sensor_msgs::msg::PointCloud2` | road surface markings converted to line segments | +| `input/ll2_road_marking` | `sensor_msgs::msg::PointCloud2` | road surface markings converted to line segments | +| `input/projected_line_segments_cloud` | `sensor_msgs::msg::PointCloud2` | projected line segments | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | reference to retrieve the area map around the self location | #### Output -| Name | Type | Description | -| ------------------------------ | -------------------------------------------- | --------------------------------------------------------- | -| `output/weighted_particles` | `yabloc_particle_filter::msg::ParticleArray` | weighted particles | -| `debug/cost_map_image` | `sensor_msgs::msg::Image` | cost map created from lanelet2 | -| `debug/cost_map_range` | `visualization_msgs::msg::MarkerArray` | cost map boundary | -| `debug/match_image` | `sensor_msgs::msg::Image` | projected line segments image | -| `debug/scored_cloud` | `sensor_msgs::msg::PointCloud2` | weighted 3d line segments | -| `debug/scored_post_cloud` | `sensor_msgs::msg::PointCloud2` | weighted 3d line segments which are iffy | -| `debug/state_string` | `std_msgs::msg::String` | string describing the node state | -| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | +| Name | Type | Description | +| ------------------------------ | ----------------------------------------------------- | --------------------------------------------------------- | +| `output/weighted_particles` | `autoware_yabloc_particle_filter::msg::ParticleArray` | weighted particles | +| `debug/cost_map_image` | `sensor_msgs::msg::Image` | cost map created from lanelet2 | +| `debug/cost_map_range` | `visualization_msgs::msg::MarkerArray` | cost map boundary | +| `debug/match_image` | `sensor_msgs::msg::Image` | projected line segments image | +| `debug/scored_cloud` | `sensor_msgs::msg::PointCloud2` | weighted 3d line segments | +| `debug/scored_post_cloud` | `sensor_msgs::msg::PointCloud2` | weighted 3d line segments which are iffy | +| `debug/state_string` | `std_msgs::msg::String` | string describing the node state | +| `debug/particles_marker_array` | `visualization_msgs::msg::MarkerArray` | particles visualization. published if `visualize` is true | ### Parameters diff --git a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp index 0710c8c4dca64..031c03b9197fc 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp @@ -44,10 +44,10 @@ double mean_radian(const std::vector & angles, const std::vector } // namespace geometry_msgs::msg::Pose get_mean_pose( - const yabloc_particle_filter::msg::ParticleArray & particle_array) + const autoware_yabloc_particle_filter::msg::ParticleArray & particle_array) { using Pose = geometry_msgs::msg::Pose; - using Particle = yabloc_particle_filter::msg::Particle; + using Particle = autoware_yabloc_particle_filter::msg::Particle; Pose mean_pose; @@ -89,9 +89,9 @@ geometry_msgs::msg::Pose get_mean_pose( return mean_pose; } -Eigen::Matrix3f std_of_distribution(const yabloc_particle_filter::msg::ParticleArray & array) +Eigen::Matrix3f std_of_distribution(const autoware_yabloc_particle_filter::msg::ParticleArray & array) { - using Particle = yabloc_particle_filter::msg::Particle; + using Particle = autoware_yabloc_particle_filter::msg::Particle; auto ori = get_mean_pose(array).orientation; Eigen::Quaternionf orientation(ori.w, ori.x, ori.y, ori.z); float invN = 1.f / array.particles.size(); @@ -113,9 +113,9 @@ Eigen::Matrix3f std_of_distribution(const yabloc_particle_filter::msg::ParticleA return sigma; } -float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array) +float std_of_weight(const autoware_yabloc_particle_filter::msg::ParticleArray & particle_array) { - using Particle = yabloc_particle_filter::msg::Particle; + using Particle = autoware_yabloc_particle_filter::msg::Particle; const float invN = 1.f / particle_array.particles.size(); float mean = 0; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index 90cd62883e339..024ec95125f35 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -25,8 +25,8 @@ namespace yabloc::modularized_particle_filter class ParticleVisualize : public rclcpp::Node { public: - using Particle = yabloc_particle_filter::msg::Particle; - using ParticleArray = yabloc_particle_filter::msg::ParticleArray; + using Particle = autoware_yabloc_particle_filter::msg::Particle; + using ParticleArray = autoware_yabloc_particle_filter::msg::ParticleArray; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; diff --git a/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp index 67ca4f5add947..2a4693b6ee695 100644 --- a/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp @@ -16,8 +16,8 @@ namespace yabloc::modularized_particle_filter { -std::optional find_synced_particles( - boost::circular_buffer circular_buffer, +std::optional find_synced_particles( + boost::circular_buffer circular_buffer, rclcpp::Time time) { for (int i{1}; i < static_cast(circular_buffer.size()); i++) { diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index b16905ba19b41..80ee96aa59cf9 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -97,7 +97,7 @@ void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose void Predictor::initialize_particles(const PoseCovStamped & initialpose) { RCLCPP_INFO_STREAM(this->get_logger(), "initialize_particles"); - yabloc_particle_filter::msg::ParticleArray particle_array{}; + autoware_yabloc_particle_filter::msg::ParticleArray particle_array{}; particle_array.header = initialpose.header; particle_array.id = 0; particle_array.particles.resize(number_of_particles_); diff --git a/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp index 124b0500e010e..7235f36119c36 100644 --- a/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp +++ b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp @@ -19,8 +19,8 @@ #include namespace mpf = yabloc::modularized_particle_filter; -using Particle = yabloc_particle_filter::msg::Particle; -using ParticleArray = yabloc_particle_filter::msg::ParticleArray; +using Particle = autoware_yabloc_particle_filter::msg::Particle; +using ParticleArray = autoware_yabloc_particle_filter::msg::ParticleArray; constexpr int PARTICLE_COUNT = 10; constexpr int HISTORY_SIZE = 10;