A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) approach.
More...
#include <AMCLLocalizer.hpp>
|
| 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.
|
|
|
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.
|
|
|
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< Particle > | particles_ |
| 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.
|
|
A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) approach.
◆ AMCLLocalizer()
◆ ~AMCLLocalizer()
◆ correct()
void correct |
( |
NavState & | nav_state | ) |
|
|
protected |
Applies the sensor model to update particle weights.
- Parameters
-
nav_state | The 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
-
msg | The 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_state | The current navigation state. |
◆ publishEstimatedPose()
void publishEstimatedPose |
( |
const tf2::Transform & | est_pose | ) |
|
|
protected |
Publishes the estimated pose with covariance.
- Parameters
-
est_pose | The 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
-
map2bf | The transform to be published. |
◆ reseed()
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_state | The 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_state | The current navigation state (read/write). |
◆ 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_
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_
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: