Solved some typos and bugs in the code, still getting stuck sometimes, but the recovery is better

Author: JesusSilvaUtrera

localization/beluga_demo_mh_amcl/config/mh_amcl_params.yaml

@@ -18,25 +18,22 @@ mh_amcl:
     min_candidate_angle: 1.047 # Equivalent to 60 degrees
 
     # Threshold for low-quality hypotheses
-    low_q_hypo_threshold: 0.15
-
-    # Threshold for very low-quality hypotheses
-    very_low_q_hypo_threshold: 0.07
+    low_q_hypo_threshold: 0.2
 
     # Distance threshold for merging similar hypotheses (in meters)
     hypo_merge_distance: 0.4
 
     # Angular threshold for merging similar hypotheses (in radians)
-    hypo_merge_angle: 0.5 # Equivalent to 30 degrees
+    hypo_merge_angle: 0.523 # Equivalent to 30 degrees
 
     # Threshold to consider a hypothesis as "good"
-    good_hypo_threshold: 0.8
+    good_hypo_threshold: 0.5
 
     # Minimum difference in quality between the best hypothesis and others
-    min_hypo_diff_winner: 0.3
+    min_hypo_diff_winner: 0.2
 
     # Bond timeout
-    bond_timeout: 4.0
+    bond_timeout: 10.0
 
     # Standard deviation for the LiDAR
     distance_perception_error: 0.05

localization/beluga_demo_mh_amcl/launch/bringup.launch.py

@@ -57,7 +57,8 @@ def generate_launch_description():
                             'map.yaml'])},
                     {'use_sim_time': True}
                     ],
-                output='screen'
+                output='screen',
+                emulate_tty=True
             ),
             # Node containing the localization functionality
             Node(
@@ -68,17 +69,20 @@ def generate_launch_description():
                 parameters=[
                     PathJoinSubstitution([pkg_mh_amcl, 'config', 'mh_amcl_params.yaml'])
                 ],
+                output='screen',
+                emulate_tty=True,
             ),
             Node(
                 package='nav2_lifecycle_manager',
                 executable='lifecycle_manager',
                 name='lifecycle_manager_localization',
-                output='screen',
                 parameters=[
                     {"autostart": True},
                     {'node_names': lifecycle_nodes},
                     {'use_sim_time': True}
-                ]
+                ],
+                output='screen',
+                emulate_tty=True
             )
         ]
     )

localization/beluga_demo_mh_amcl/src/mh_amcl/mh_amcl.cpp

@@ -338,6 +338,9 @@ namespace mh_amcl
     }
 
     last_time_ = last_laser_->header.stamp;
+
+    RCLCPP_DEBUG_STREAM(get_logger(),
+                        "Correct [" << (now() - start).seconds() << " secs]");
   }
 
   void MH_AMCL_Node::reseed()
@@ -348,6 +351,9 @@ namespace mh_amcl
     {
       particles->reseed();
     }
+
+    RCLCPP_DEBUG_STREAM(get_logger(),
+                        "Reseed [" << (now() - start).seconds() << " secs]");
   }
 
   void MH_AMCL_Node::initpose_callback(
@@ -545,25 +551,26 @@ namespace mh_amcl
       }
     }
 
-    // Select the best hypothesis
-    current_hypothesis_ = *std::max_element(particles_population_.begin(), particles_population_.end(),
-                                            [](const auto &a, const auto &b)
-                                            { return a->get_quality() < b->get_quality(); });
-    current_hypothesis_q_ = current_hypothesis_->get_quality();
+    // Find the best hypothesis currently in the population.
+    auto best_it = std::max_element(particles_population_.begin(), particles_population_.end(),
+                                    [](const auto &a, const auto &b)
+                                    { return a->get_quality() < b->get_quality(); });
+    auto best_hypothesis = *best_it;
 
-    if (current_hypothesis_q_ < good_hypo_threshold_)
+    // Only change if it's significantly better
+    if (best_hypothesis->get_quality() > current_hypothesis_q_ + min_hypo_diff_winner_)
     {
-      current_hypothesis_ = particles_population_.front();
-      current_hypothesis_q_ = current_hypothesis_->get_quality();
+      current_hypothesis_ = best_hypothesis;
     }
+    current_hypothesis_q_ = current_hypothesis_->get_quality();
 
     // Debug the output
-    std::cout << "=====================================" << std::endl;
+    RCLCPP_INFO(get_logger(), "====== Hypotheses Status ======");
     for (const auto &amcl : particles_population_)
     {
-      std::cout << (amcl == current_hypothesis_ ? "->\t" : "") << amcl->get_quality() << std::endl;
+      RCLCPP_INFO(get_logger(), "%s Quality: %f", (amcl == current_hypothesis_ ? "-> " : "   "), amcl->get_quality());
     }
-    std::cout << "=====================================" << std::endl;
+    RCLCPP_INFO(get_logger(), "==============================");
   }
 
   signed char MH_AMCL_Node::get_cost(const geometry_msgs::msg::Pose &pose)

localization/beluga_demo_mh_amcl/src/mh_amcl/particles_distribution.cpp

@@ -15,14 +15,7 @@
 #include <algorithm>
 #include <cmath>
 #include <numeric>
-
-#include <beluga_ros/particle_cloud.hpp>
-
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <tf2_ros/buffer_interface.h>
-#include <visualization_msgs/msg/marker_array.hpp>
-
-#include "beluga_demo_mh_amcl/particles_distribution.hpp"
+#include <range/v3/view.hpp>
 
 #include <beluga/random/multivariate_normal_distribution.hpp>
 #include <beluga/actions/propagate.hpp>
@@ -32,9 +25,13 @@
 #include <beluga/algorithm/estimation.hpp>
 #include <beluga/motion/differential_drive_model.hpp>
 #include <beluga/views/sample.hpp>
-#include <beluga/views/particles.hpp>
 
-#include <range/v3/view.hpp>
+#include <beluga_ros/particle_cloud.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <tf2_ros/buffer_interface.h>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include "beluga_demo_mh_amcl/particles_distribution.hpp"
 
 namespace mh_amcl
 {
@@ -110,9 +107,9 @@ namespace mh_amcl
     {
       parent_node->declare_parameter("good_hypo_threshold", 0.6);
     }
-    if (!parent_node->has_parameter("good_hypo_threshold"))
+    if (!parent_node->has_parameter("low_q_hypo_threshold"))
     {
-      parent_node->declare_parameter("low_q_hypo_threshold", 0.25f);
+      parent_node->declare_parameter("low_q_hypo_threshold", 0.25);
     }
     if (!parent_node->has_parameter("particles_step"))
     {
@@ -354,11 +351,11 @@ namespace mh_amcl
       std::shared_ptr<beluga_ros::OccupancyGrid> costmap)
   {
     std::string error;
-    if (tf_buffer_.canTransform(scan.header.frame_id, "base_footprint",
+    if (tf_buffer_.canTransform("base_footprint", scan.header.frame_id,
                                 tf2_ros::fromMsg(scan.header.stamp), &error))
     {
       auto bf2laser_msg =
-          tf_buffer_.lookupTransform(scan.header.frame_id, "base_footprint",
+          tf_buffer_.lookupTransform("base_footprint", scan.header.frame_id,
                                      tf2_ros::fromMsg(scan.header.stamp));
       tf2::fromMsg(bf2laser_msg, bf2laser_);
     }
@@ -370,52 +367,77 @@ namespace mh_amcl
       return;
     }
 
-    const double normal_comp_1 = utils::INV_SQRT_2PI / distance_perception_error_;
-
-    for (auto &p : particles_)
+    // ! Review sensor likelihood math and code
+    // Build a sensor-model callable: given a Particle, returns a weight (likelihood)
+    auto sensor_likelihood = [this, &scan, &costmap](const Sophus::SE2d &state) -> double
     {
-      p.hits = 0.0;
-    }
+      // Convert state -> tf2 transform or world coords needed for your existing ray-cast:
+      tf2::Transform map2bf = utils::se2dToTf2Transform(state);
 
-    for (int j = 0; j < scan.ranges.size(); j++)
-    {
-      if (std::isnan(scan.ranges[j]) || std::isinf(scan.ranges[j]))
+      // Loop over the scan to get the accumulated logarithmic likelihood and the valid beams (those that don't return infinite)
+      double accum_log_likelihood = 0.0;
+      int valid_beams = 0;
+      // You may want to compute per-particle likelihood by summing log-likelihoods over beams
+      for (size_t j = 0u; j < scan.ranges.size(); ++j)
       {
-        continue;
-      }
+        double r = scan.ranges[j];
+        if (!std::isfinite(r))
+          continue;
 
-      tf2::Transform laser2point = get_transform_to_read(scan, j);
+        tf2::Transform laser2point = get_transform_to_read(scan, static_cast<int>(j));
+        double err_m = get_error_distance_to_obstacle(map2bf, bf2laser_, laser2point, scan, costmap, distance_perception_error_);
+        if (!std::isinf(err_m))
+        {
+          double a = err_m / distance_perception_error_;
+          double l = std::log(utils::INV_SQRT_2PI / distance_perception_error_) + (-0.5 * a * a);
+          accum_log_likelihood += l;
+          ++valid_beams;
+        }
+      }
 
-      for (int i = 0; i < particles_.size(); i++)
+      if (valid_beams == 0)
       {
-        auto &p = particles_[i];
+        // fallback small weight
+        return 1e-9;
+      }
+      // Convert back from log-likelihood to linear scale (might underflow — you can return exp(accum_log_likelihood/valid_beams))
+      double avg_log_l = accum_log_likelihood / static_cast<double>(valid_beams);
+      double likelihood = std::exp(avg_log_l);
+      return likelihood;
+    };
 
-        tf2::Transform particle_pose_tf2 = utils::se2dToTf2Transform(p.state);
+    // Apply Beluga reweight action and normalize
+    particles_ |= beluga::actions::reweight(sensor_likelihood) | beluga::actions::normalize();
 
-        double calculated_distance = get_error_distance_to_obstacle(
-            particle_pose_tf2, bf2laser_, laser2point, scan, costmap,
-            distance_perception_error_);
+    // ! Study if this can be included somehow in the same loop as the likelihood (probably not because Beluga's reweight action receives a state, not the whole particle)
+    // Calculate quality based on the average probability of hits, as per the original implementation.
+    // This is calculated separately from the particle weights for robustness.
+    quality_ = 0.0;
+    for (auto &p : particles_)
+    {
+      tf2::Transform map2bf = utils::se2dToTf2Transform(p.state);
+      double total_prob = 0.0;
+      for (size_t j = 0u; j < scan.ranges.size(); ++j)
+      {
+        if (std::isnan(scan.ranges[j]) || std::isinf(scan.ranges[j]))
+        {
+          continue;
+        }
 
-        if (!std::isinf(calculated_distance))
+        tf2::Transform laser2point = get_transform_to_read(scan, static_cast<int>(j));
+        double err_m = get_error_distance_to_obstacle(map2bf, bf2laser_, laser2point, scan, costmap, distance_perception_error_);
+
+        if (!std::isinf(err_m))
         {
-          const double a = calculated_distance / distance_perception_error_;
+          const double o = distance_perception_error_;
+          const double normal_comp_1 = utils::INV_SQRT_2PI / o;
+          const double a = err_m / o;
           const double normal_comp_2 = std::exp(-0.5 * a * a);
-
           double prob = std::clamp(normal_comp_1 * normal_comp_2, 0.0, 1.0);
-          p.weight = std::max(p.weight + prob, 0.000001);
-
-          p.hits += prob;
+          total_prob += prob;
         }
       }
-    }
-
-    particles_ |= beluga::actions::normalize();
-
-    // Calculate quality
-    quality_ = 0.0;
-    for (auto &p : particles_)
-    {
-      p.hits = p.hits / static_cast<float>(scan.ranges.size());
+      p.hits = total_prob / static_cast<float>(scan.ranges.size());
       quality_ = std::max(quality_, p.hits);
     }
   }