AMCLLocalizer.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

 
#ifndef EASYNAV_COSTMAP_LOCALIZER__AMCLLOCALIZER_HPP_
#define EASYNAV_COSTMAP_LOCALIZER__AMCLLOCALIZER_HPP_
 
#include <vector>
#include <random>
#include <Eigen/Geometry>
 
#include "geometry_msgs/msg/pose_array.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
 
#include "tf2/LinearMath/Transform.hpp"
#include "tf2_ros/transform_broadcaster.hpp"
 
#include "easynav_core/LocalizerMethodBase.hpp"
 
namespace easynav
{

struct Particle
{
  tf2::Transform pose;      
  int hits;                 
  int possible_hits;        
  double weight;            
};

class AMCLLocalizer : public LocalizerMethodBase
{
public:
  AMCLLocalizer();

  ~AMCLLocalizer();

  virtual void on_initialize() override;

  void update_rt(NavState & nav_state) override;

  void update(NavState & nav_state) override;

  tf2::Transform getEstimatedPose() const;

  nav_msgs::msg::Odometry get_pose();
 
protected:
  void initializeParticles();

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

  void publishParticles();

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

  void predict(NavState & nav_state);

  void correct(NavState & nav_state);

  void reseed();

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

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

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

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

  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr init_pose_sub_;

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

  void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg);
 

  void update_odom_from_tf();

  std::vector<Particle> particles_;

  std::mt19937 rng_;

  nav_msgs::msg::Odometry pose_;

  double noise_translation_ {0.01};

  double noise_rotation_ {0.01};

  double noise_translation_to_rotation_ {0.01};

  double min_noise_xy_ {0.05};

  double min_noise_yaw_ {0.05};

  bool compute_odom_from_tf_ {false};

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

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

  bool initialized_odom_ = false;

  double reseed_time_;

  rclcpp::Time last_input_time_;

  rclcpp::Time last_reseed_;
};
 
}  // namespace easynav
 
#endif  // EASYNAV_COSTMAP_LOCALIZER__AMCLLOCALIZER_HPP_