yabLoc_particle_filter#

This package contains some executable nodes related to particle filter.

particle_predictor#

This node performs predictive updating and resampling of particles.
It retroactively reflects the particle weights determined by the corrector node.

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
`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	Default	Range
visualize	boolean	whether particles are also published in visualization_msgs or not	True	N/A
static_linear_covariance	float	overriding covariance of `/twist_with_covariance`	0.04	N/A
static_angular_covariance	float	overriding covariance of `/twist_with_covariance`	0.006	N/A
resampling_interval_seconds	float	the interval of particle resampling	1.0	N/A
num_of_particles	float	the number of particles	500	N/A
prediction_rate	float	frequency of forecast updates, in Hz	50.0	N/A
cov_xx_yy	array	the covariance of initial pose	[2.0, 0.25]	N/A

Name	Type	Description
`yabloc_trigger_srv`	`std_srvs::srv::SetBool`	activation and deactivation of yabloc estimation

This node estimated particles weight using GNSS.
It supports two types of input: ublox_msgs::msg::NavPVT and geometry_msgs::msg::PoseWithCovarianceStamped.

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
`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	Default	Range
acceptable_max_delay	float	how long to hold the predicted particles	1	N/A
visualize	boolean	whether publish particles as marker_array or not	0	N/A
mahalanobis_distance_threshold	float	if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips.	30	N/A
for_fixed/max_weight	float	gnss weight distribution used when observation is fixed	5	N/A
for_fixed/flat_radius	float	gnss weight distribution used when observation is fixed	0.5	N/A
for_fixed/max_radius	float	gnss weight distribution used when observation is fixed	10	N/A
for_fixed/min_weight	float	gnss weight distribution used when observation is fixed	0.5	N/A
for_not_fixed/max_weight	float	gnss weight distribution used when observation is not fixed	1	N/A
for_not_fixed/flat_radius	float	gnss weight distribution used when observation is not fixed	5	N/A
for_not_fixed/max_radius	float	gnss weight distribution used when observation is not fixed	20	N/A
for_not_fixed/min_weight	float	gnss weight distribution used when observation is not fixed	0.5	N/A

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
`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	Default	Range
acceptable_max_delay	float	how long to hold the predicted particles	1	N/A
visualize	boolean	whether publish particles as marker_array or not	0	N/A
image_size	float	image size of debug/cost_map_image	800	N/A
max_range	float	width of hierarchical cost map	40	N/A
gamma	float	gamma value of the intensity gradient of the cost map	5	N/A
min_prob	float	minimum particle weight the corrector node gives	0.1	N/A
far_weight_gain	float	`exp(-far_weight_gain_ * squared_distance_from_camera)` is weight gain. if this is large, the nearby road markings will be more important	0.001	N/A
enabled_at_first	boolean	if it is false, this node is not activated at first. you can activate by service call	1	N/A

Name	Type	Description
`switch_srv`	`std_srvs::srv::SetBool`	activation and deactivation of correction