Skip to content

Commit

Permalink
build(yabloc_particle_filter): fix namespace for messages. Fix format…
Browse files Browse the repository at this point in the history 
…ting

Signed-off-by: Esteve Fernandez <[email protected]>
  • Loading branch information
@esteve
esteve committed Jan 24, 2024
1 parent cf5a778 commit a366c42
Show file tree
Hide file tree
Showing 6 changed files with 54 additions and 54 deletions.
82 changes: 41 additions & 41 deletions localization/yabloc/yabloc_particle_filter/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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

Expand Down Expand Up @@ -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

Expand Down
12 changes: 6 additions & 6 deletions localization/yabloc/yabloc_particle_filter/src/common/mean.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,10 @@ double mean_radian(const std::vector<double> & angles, const std::vector<double>
} // 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;

Expand Down Expand Up @@ -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();
Expand All @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
4 changes: 2 additions & 2 deletions localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@

namespace yabloc::modularized_particle_filter
{
std::optional<yabloc_particle_filter::msg::ParticleArray> find_synced_particles(
boost::circular_buffer<yabloc_particle_filter::msg::ParticleArray> circular_buffer,
std::optional<autoware_yabloc_particle_filter::msg::ParticleArray> find_synced_particles(
boost::circular_buffer<autoware_yabloc_particle_filter::msg::ParticleArray> circular_buffer,
rclcpp::Time time)
{
for (int i{1}; i < static_cast<int>(circular_buffer.size()); i++) {
Expand Down
2 changes: 1 addition & 1 deletion localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down
4 changes: 2 additions & 2 deletions localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@
#include <gtest/gtest.h>

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;
Expand Down

0 comments on commit a366c42

Please sign in to comment.