EasyNav Simple Stack
Loading...
Searching...
No Matches
AMCLLocalizer.hpp
Go to the documentation of this file.
1// Copyright 2025 Intelligent Robotics Lab
2//
3// This file is part of the project Easy Navigation (EasyNav in short)
4// licensed under the GNU General Public License v3.0.
5// See <http://www.gnu.org/licenses/> for details.
6//
7// Easy Navigation program is free software: you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation, either version 3 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License
18// along with this program. If not, see <http://www.gnu.org/licenses/>.
19
22
23#ifndef EASYNAV_SIMPLE_LOCALIZER__AMCLLOCALIZER_HPP_
24#define EASYNAV_SIMPLE_LOCALIZER__AMCLLOCALIZER_HPP_
25
26#include <vector>
27#include <stdexcept>
28#include <algorithm>
29#include <utility>
30#include <fstream>
31#include <sstream>
32#include <random>
33#include <Eigen/Geometry>
34
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"
38
39#include "tf2/LinearMath/Transform.hpp"
40#include "tf2_ros/transform_broadcaster.h"
41
42#include "easynav_core/LocalizerMethodBase.hpp"
43
44namespace easynav
45{
46
49{
50 tf2::Transform pose;
51 int hits;
53 double weight;
54};
55
58{
59public:
64
69
77 virtual std::expected<void, std::string> on_initialize() override;
78
86 void update_rt(NavState & nav_state) override;
87
95 void update(NavState & nav_state) override;
96
102 tf2::Transform getEstimatedPose() const;
103
109 nav_msgs::msg::Odometry get_pose();
110
111protected:
116
122 void publishTF(const tf2::Transform & map2bf);
123
127 void publishParticles();
128
134 void publishEstimatedPose(const tf2::Transform & est_pose);
135
141 void predict(NavState & nav_state);
142
148 void correct(NavState & nav_state);
149
153 void reseed();
154
156 std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
157
159 rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr particles_pub_;
160
162 rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr estimate_pub_;
163
165 rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
166
172 void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg);
173
175 std::vector<Particle> particles_;
176
178 std::default_random_engine rng_;
179
181 nav_msgs::msg::Odometry pose_;
182
184 double noise_translation_ {0.01};
185
187 double noise_rotation_ {0.01};
188
191
193 double min_noise_xy_ {0.05};
194
196 double min_noise_yaw_ {0.05};
197
199 tf2::Transform odom_{tf2::Transform::getIdentity()};
200
202 tf2::Transform last_odom_{tf2::Transform::getIdentity()};
203
205 bool initialized_odom_ = false;
206
209
211 rclcpp::Time last_reseed_;
212};
213
214} // namespace easynav
215
216#endif // EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_
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