23#ifndef EASYNAV_SIMPLE_LOCALIZER__AMCLLOCALIZER_HPP_
24#define EASYNAV_SIMPLE_LOCALIZER__AMCLLOCALIZER_HPP_
33#include <Eigen/Geometry>
35#include "geometry_msgs/msg/pose_array.hpp"
36#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
37#include "nav_msgs/msg/odometry.hpp"
39#include "tf2/LinearMath/Transform.hpp"
40#include "tf2_ros/transform_broadcaster.h"
42#include "easynav_core/LocalizerMethodBase.hpp"
77 virtual std::expected<void, std::string>
on_initialize()
override;
86 void update_rt(NavState & nav_state)
override;
95 void update(NavState & nav_state)
override;
122 void publishTF(
const tf2::Transform & map2bf);
141 void predict(NavState & nav_state);
148 void correct(NavState & nav_state);
162 rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
estimate_pub_;
165 rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr
odom_sub_;
178 std::default_random_engine
rng_;
199 tf2::Transform
odom_{tf2::Transform::getIdentity()};
double min_noise_yaw_
Minimum yaw noise threshold.
Definition AMCLLocalizer.hpp:196
void update(NavState &nav_state) override
General update of the localization state.
Definition AMCLLocalizer.cpp:297
double min_noise_xy_
Minimum translation noise threshold.
Definition AMCLLocalizer.hpp:193
nav_msgs::msg::Odometry pose_
Current estimated odometry-based pose.
Definition AMCLLocalizer.hpp:181
std::unique_ptr< tf2_ros::TransformBroadcaster > tf_broadcaster_
TF broadcaster to publish map to base_footprint transform.
Definition AMCLLocalizer.hpp:156
std::vector< Particle > particles_
List of particles representing the belief distribution.
Definition AMCLLocalizer.hpp:175
~AMCLLocalizer()
Destructor.
Definition AMCLLocalizer.cpp:185
rclcpp::Time last_reseed_
Timestamp of the last reseed event.
Definition AMCLLocalizer.hpp:211
double reseed_time_
Time interval (in seconds) after which the particles should be reseeded.
Definition AMCLLocalizer.hpp:208
rclcpp::Publisher< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr estimate_pub_
Publisher for the estimated robot pose with covariance.
Definition AMCLLocalizer.hpp:162
void update_rt(NavState &nav_state) override
Real-time update of the localization state.
Definition AMCLLocalizer.cpp:289
void publishEstimatedPose(const tf2::Transform &est_pose)
Publishes the estimated pose with covariance.
Definition AMCLLocalizer.cpp:591
rclcpp::Subscription< nav_msgs::msg::Odometry >::SharedPtr odom_sub_
Subscriber for odometry messages.
Definition AMCLLocalizer.hpp:165
double noise_translation_to_rotation_
Coupling noise between translation and rotation.
Definition AMCLLocalizer.hpp:190
void publishTF(const tf2::Transform &map2bf)
Publishes a TF transform between map and base footprint.
Definition AMCLLocalizer.cpp:526
void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg)
Callback for receiving odometry updates.
Definition AMCLLocalizer.cpp:312
tf2::Transform odom_
Last odometry transform received.
Definition AMCLLocalizer.hpp:199
tf2::Transform last_odom_
Previous odometry transform (used to compute deltas).
Definition AMCLLocalizer.hpp:202
virtual std::expected< void, std::string > on_initialize() override
Initializes the localization method.
Definition AMCLLocalizer.cpp:190
void predict(NavState &nav_state)
Applies the motion model to update particle poses.
Definition AMCLLocalizer.cpp:323
nav_msgs::msg::Odometry get_pose()
Gets the current estimated pose as an Odometry message.
Definition AMCLLocalizer.cpp:622
double noise_rotation_
Rotational noise standard deviation.
Definition AMCLLocalizer.hpp:187
std::default_random_engine rng_
Random number generator used for sampling noise.
Definition AMCLLocalizer.hpp:178
rclcpp::Publisher< geometry_msgs::msg::PoseArray >::SharedPtr particles_pub_
Publisher for visualization of the particle cloud.
Definition AMCLLocalizer.hpp:159
void publishParticles()
Publishes the current set of particles.
Definition AMCLLocalizer.cpp:539
tf2::Transform getEstimatedPose() const
Gets the current estimated pose as a transform.
Definition AMCLLocalizer.cpp:562
double noise_translation_
Translational noise standard deviation.
Definition AMCLLocalizer.hpp:184
bool initialized_odom_
Flag indicating if the odometry has been initialized.
Definition AMCLLocalizer.hpp:205
void reseed()
Re-initializes the particle cloud if necessary.
Definition AMCLLocalizer.cpp:449
void correct(NavState &nav_state)
Applies the sensor model to update particle weights.
Definition AMCLLocalizer.cpp:374
AMCLLocalizer()
Default constructor.
Definition AMCLLocalizer.cpp:163
void initializeParticles()
Initializes the set of particles.
Definition SimpleMap.hpp:37
Structure representing a single particle in the AMCL algorithm.
Definition AMCLLocalizer.hpp:49
int hits
Number of sensor matches (hits) for this particle.
Definition AMCLLocalizer.hpp:51
double weight
Normalized importance weight of the particle.
Definition AMCLLocalizer.hpp:53
int possible_hits
Maximum number of possible hits.
Definition AMCLLocalizer.hpp:52
tf2::Transform pose
Estimated pose of the particle.
Definition AMCLLocalizer.hpp:50