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_NAVMAP_LOCALIZER__AMCLLOCALIZER_HPP_
#define EASYNAV_NAVMAP_LOCALIZER__AMCLLOCALIZER_HPP_
 
#include <vector>
#include <random>
#include <Eigen/Geometry>
#include <bonxai/probabilistic_map.hpp>
 
#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 "navmap_core/NavMap.hpp"
 
#include "easynav_core/LocalizerMethodBase.hpp"
 
namespace easynav
{
namespace navmap
{

struct Particle
{
  tf2::Transform pose;      
  float hits;                 
  float possible_hits;        
  double weight;            
 
  ::navmap::NavCelId last_cid = std::numeric_limits<uint32_t>::max();
  std::size_t last_surface = 0;
};

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_;

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

  void update_odom_from_tf();

  std::vector<Particle> particles_;

  std::mt19937 rng_;

  tf2::Transform 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};
 
  double inflation_stddev_   {1.5};     // meters
  double inflation_prob_min_ {0.01};    // [0..1] min prob kept in inflated map
  std::size_t correct_max_points_ {1500}; // hard cap of points per sensor cloud
  double weights_tau_ {0.7};            // <1 sharpens, >1 flattens
  // Fraction of particles (0,1] used as "top" set for stats and reseed
  double top_keep_fraction_{0.2};
  double downsampled_cloud_size_{0.05};

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

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

  bool initialized_odom_ = false;

  double reseed_time_;

  rclcpp::Time last_reseed_;

  rclcpp::Time last_input_time_;

  std::shared_ptr<Bonxai::ProbabilisticMap> bonxai_map_;
  ::navmap::NavCelId last_cid_ {0};
 
  // PerceptionModel percepcion_model_;
};
 
}  // namespace navmap
 
}  // namespace easynav
#endif  // EASYNAV_NAVMAP_LOCALIZER__AMCLLOCALIZER_HPP_