Class AMCLLocalizer

Inheritance Relationships

Base Type

  • public LocalizerMethodBase

Class Documentation

class AMCLLocalizer : public LocalizerMethodBase

A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) approach.

Public Functions

AMCLLocalizer()

Default constructor.

~AMCLLocalizer()

Destructor.

virtual void on_initialize() override

Initializes the localization method.

Sets up publishers, subscribers, and prepares the particle filter.

Throws:

std::runtime_error – if initialization fails.

void update_rt(NavState &nav_state) override

Real-time update of the localization state.

Used for time-critical update operations.

Parameters:

nav_state – The current navigation state (read/write).

void update(NavState &nav_state) override

General update of the localization state.

May include operations not suitable for real-time execution.

Parameters:

nav_state – The current navigation state (read/write).

tf2::Transform getEstimatedPose() const

Gets the current estimated pose as a transform.

Returns:

The transform from map to base footprint frame.

nav_msgs::msg::Odometry get_pose()

Gets the current estimated pose as an Odometry message.

Returns:

A nav_msgs::msg::Odometry message containing the estimated pose.

Protected Functions

void initializeParticles()

Initializes the set of particles.

void publishTF(const tf2::Transform &map2bf)

Publishes a TF transform between map and base footprint.

Parameters:

map2bf – The transform to be published.

void publishParticles()

Publishes the current set of particles.

void publishEstimatedPose(const tf2::Transform &est_pose)

Publishes the estimated pose with covariance.

Parameters:

est_pose – The estimated transform to be published.

void predict(NavState &nav_state)

Applies the motion model to update particle poses.

Parameters:

nav_state – The current navigation state.

void correct(NavState &nav_state)

Applies the sensor model to update particle weights.

Parameters:

nav_state – The current navigation state.

void reseed()

Re-initializes the particle cloud if necessary.

void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg)

Callback for receiving odometry updates.

Parameters:

msg – The incoming odometry message.

void update_odom_from_tf()

Update odom from TFs instead of a odom topic.

Protected Attributes

std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_

TF broadcaster to publish map to base_footprint transform.

rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr particles_pub_

Publisher for visualization of the particle cloud.

rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr estimate_pub_

Publisher for the estimated robot pose with covariance.

rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_

Subscriber for odometry messages.

std::vector<Particle> particles_

List of particles representing the belief distribution.

std::mt19937 rng_

Random number generator used for sampling noise.

tf2::Transform pose_

Current estimated odometry-based pose.

double noise_translation_ = {0.01}

Translational noise standard deviation.

double noise_rotation_ = {0.01}

Rotational noise standard deviation.

double noise_translation_to_rotation_ = {0.01}

Coupling noise between translation and rotation.

double min_noise_xy_ = {0.05}

Minimum translation noise threshold.

double min_noise_yaw_ = {0.05}

Minimum yaw noise threshold.

bool compute_odom_from_tf_ = {false}

Whether to use TFs to compute odom.

double inflation_stddev_ = {1.5}
double inflation_prob_min_ = {0.01}
std::size_t correct_max_points_ = {1500}
double weights_tau_ = {0.7}
double top_keep_fraction_ = {0.2}
double downsampled_cloud_size_ = {0.05}
tf2::Transform odom_ = {tf2::Transform::getIdentity()}

Last odometry transform received.

tf2::Transform last_odom_ = {tf2::Transform::getIdentity()}

Previous odometry transform (used to compute deltas).

bool initialized_odom_ = false

Flag indicating if the odometry has been initialized.

double reseed_time_

Time interval (in seconds) after which the particles should be reseeded.

rclcpp::Time last_reseed_

Timestamp of the last reseed event.

rclcpp::Time last_input_time_

Timestamp of the last input message (odometry or initial pose).

std::shared_ptr<Bonxai::ProbabilisticMap> bonxai_map_

Internal static map.

::navmap::NavCelId last_cid_ = {0}