EasyNav Simple Stack
Loading...
Searching...
No Matches
AMCLLocalizer Class Reference

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

#include <AMCLLocalizer.hpp>

Inheritance diagram for AMCLLocalizer:
Collaboration diagram for AMCLLocalizer:

Public Member Functions

 AMCLLocalizer ()
 Default constructor.
 
nav_msgs::msg::Odometry get_pose ()
 Gets the current estimated pose as an Odometry message.
 
tf2::Transform getEstimatedPose () const
 Gets the current estimated pose as a transform.
 
virtual std::expected< void, std::string > on_initialize () override
 Initializes the localization method.
 
void update (NavState &nav_state) override
 General update of the localization state.
 
void update_rt (NavState &nav_state) override
 Real-time update of the localization state.
 
 ~AMCLLocalizer ()
 Destructor.
 

Protected Member Functions

void correct (NavState &nav_state)
 Applies the sensor model to update particle weights.
 
void initializeParticles ()
 Initializes the set of particles.
 
void odom_callback (nav_msgs::msg::Odometry::UniquePtr msg)
 Callback for receiving odometry updates.
 
void predict (NavState &nav_state)
 Applies the motion model to update particle poses.
 
void publishEstimatedPose (const tf2::Transform &est_pose)
 Publishes the estimated pose with covariance.
 
void publishParticles ()
 Publishes the current set of particles.
 
void publishTF (const tf2::Transform &map2bf)
 Publishes a TF transform between map and base footprint.
 
void reseed ()
 Re-initializes the particle cloud if necessary.
 

Protected Attributes

rclcpp::Publisher< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr estimate_pub_
 Publisher for the estimated robot pose with covariance.
 
bool initialized_odom_ = false
 Flag indicating if the odometry has been initialized.
 
tf2::Transform last_odom_ {tf2::Transform::getIdentity()}
 Previous odometry transform (used to compute deltas).
 
rclcpp::Time last_reseed_
 Timestamp of the last reseed event.
 
double min_noise_xy_ {0.05}
 Minimum translation noise threshold.
 
double min_noise_yaw_ {0.05}
 Minimum yaw noise threshold.
 
double noise_rotation_ {0.01}
 Rotational noise standard deviation.
 
double noise_translation_ {0.01}
 Translational noise standard deviation.
 
double noise_translation_to_rotation_ {0.01}
 Coupling noise between translation and rotation.
 
tf2::Transform odom_ {tf2::Transform::getIdentity()}
 Last odometry transform received.
 
rclcpp::Subscription< nav_msgs::msg::Odometry >::SharedPtr odom_sub_
 Subscriber for odometry messages.
 
std::vector< Particleparticles_
 List of particles representing the belief distribution.
 
rclcpp::Publisher< geometry_msgs::msg::PoseArray >::SharedPtr particles_pub_
 Publisher for visualization of the particle cloud.
 
nav_msgs::msg::Odometry pose_
 Current estimated odometry-based pose.
 
double reseed_time_
 Time interval (in seconds) after which the particles should be reseeded.
 
std::default_random_engine rng_
 Random number generator used for sampling noise.
 
std::unique_ptr< tf2_ros::TransformBroadcaster > tf_broadcaster_
 TF broadcaster to publish map to base_footprint transform.
 

Detailed Description

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

Constructor & Destructor Documentation

◆ AMCLLocalizer()

Default constructor.

◆ ~AMCLLocalizer()

Destructor.

Member Function Documentation

◆ correct()

void correct ( NavState & nav_state)
protected

Applies the sensor model to update particle weights.

Parameters
nav_stateThe current navigation state.

◆ get_pose()

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.

◆ getEstimatedPose()

tf2::Transform getEstimatedPose ( ) const

Gets the current estimated pose as a transform.

Returns
The transform from map to base footprint frame.

◆ initializeParticles()

void initializeParticles ( )
protected

Initializes the set of particles.

◆ odom_callback()

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

Callback for receiving odometry updates.

Parameters
msgThe incoming odometry message.

◆ on_initialize()

std::expected< void, std::string > on_initialize ( )
overridevirtual

Initializes the localization method.

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

Returns
std::expected<void, std::string> Success or error message.

◆ predict()

void predict ( NavState & nav_state)
protected

Applies the motion model to update particle poses.

Parameters
nav_stateThe current navigation state.

◆ publishEstimatedPose()

void publishEstimatedPose ( const tf2::Transform & est_pose)
protected

Publishes the estimated pose with covariance.

Parameters
est_poseThe estimated transform to be published.

◆ publishParticles()

void publishParticles ( )
protected

Publishes the current set of particles.

◆ publishTF()

void publishTF ( const tf2::Transform & map2bf)
protected

Publishes a TF transform between map and base footprint.

Parameters
map2bfThe transform to be published.

◆ reseed()

void reseed ( )
protected

Re-initializes the particle cloud if necessary.

◆ update()

void update ( NavState & nav_state)
override

General update of the localization state.

May include operations not suitable for real-time execution.

Parameters
nav_stateThe current navigation state (read/write).

◆ update_rt()

void update_rt ( NavState & nav_state)
override

Real-time update of the localization state.

Used for time-critical update operations.

Parameters
nav_stateThe current navigation state (read/write).

Member Data Documentation

◆ estimate_pub_

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

Publisher for the estimated robot pose with covariance.

◆ initialized_odom_

bool initialized_odom_ = false
protected

Flag indicating if the odometry has been initialized.

◆ last_odom_

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

Previous odometry transform (used to compute deltas).

◆ last_reseed_

rclcpp::Time last_reseed_
protected

Timestamp of the last reseed event.

◆ min_noise_xy_

double min_noise_xy_ {0.05}
protected

Minimum translation noise threshold.

◆ min_noise_yaw_

double min_noise_yaw_ {0.05}
protected

Minimum yaw noise threshold.

◆ noise_rotation_

double noise_rotation_ {0.01}
protected

Rotational noise standard deviation.

◆ noise_translation_

double noise_translation_ {0.01}
protected

Translational noise standard deviation.

◆ noise_translation_to_rotation_

double noise_translation_to_rotation_ {0.01}
protected

Coupling noise between translation and rotation.

◆ odom_

tf2::Transform odom_ {tf2::Transform::getIdentity()}
protected

Last odometry transform received.

◆ odom_sub_

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

Subscriber for odometry messages.

◆ particles_

std::vector<Particle> particles_
protected

List of particles representing the belief distribution.

◆ particles_pub_

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

Publisher for visualization of the particle cloud.

◆ pose_

nav_msgs::msg::Odometry pose_
protected

Current estimated odometry-based pose.

◆ reseed_time_

double reseed_time_
protected

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

◆ rng_

std::default_random_engine rng_
protected

Random number generator used for sampling noise.

◆ tf_broadcaster_

std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_
protected

TF broadcaster to publish map to base_footprint transform.


The documentation for this class was generated from the following files: