Addressed comments and studied new approach for weight and hits

Signed-off-by: JesusSilvaUtrera <jesus.silva@ekumenlabs.com>

Author: JesusSilvaUtrera

docker/images/humble/Dockerfile

@@ -23,10 +23,10 @@ FROM ros:humble-ros-base-jammy AS openvdb-builder
 
 RUN apt-get update \
   && apt-get install --no-install-recommends -y \
-  git \
-  libboost-all-dev \
-  libtbb-dev \
-  libblosc-dev \
+    git \
+    libboost-all-dev \
+    libtbb-dev \
+    libblosc-dev \
   && rm -rf /var/lib/apt/lists/*
 
 WORKDIR /opt
@@ -66,16 +66,16 @@ ENV DEBIAN_FRONTEND=noninteractive
 
 RUN apt-get update \
   && apt-get install --no-install-recommends -y \
-  ccache \
-  curl \
-  gdb \
-  gnupg \
-  git \
-  python3-pip \
-  python3-venv \
-  mc \
-  lsb-release \
-  tmux \
+    ccache \
+    curl \
+    gdb \
+    gnupg \
+    git \
+    python3-pip \
+    python3-venv \
+    mc \
+    lsb-release \
+    tmux \
   && rm -rf /var/lib/apt/lists/*
 
 ARG GZ_VERSION="harmonic"
@@ -186,18 +186,18 @@ RUN mkdir -p ~/.gazebo/models \
 # Cache Gazebo Fuel models
 COPY --from=gz_model_cacher --chown=$USER:$GROUP /tmp/ws/ $USER_WS/
 RUN find src -name '*.world' -or -name '*.sdf' | \
-  grep -Roh -e 'https://fuel.gazebosim.org[^<]*' src | \
-  sort | uniq | xargs -I{} gz fuel download -u {}
+    grep -Roh -e 'https://fuel.gazebosim.org[^<]*' src | \
+    sort | uniq | xargs -I{} gz fuel download -u {}
 
 ENV GZ_SIM_RESOURCE_PATH=/home/$USER/.gazebo/models
 
 #
 # Install OpenVDB files
 COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/openvdb/ /opt/openvdb
 RUN cd /opt/openvdb/build \
-  && sudo make install \
-  && sudo rm -rf /opt/openvdb
+    && sudo make install \
+    && sudo rm -rf /opt/openvdb
 RUN sudo dpkg -r libopenvdb8.1 \
-  libopenvdb-dev
+                 libopenvdb-dev
 
-ENTRYPOINT ["fixuid", "-q", "/ros_entrypoint.sh", "/bin/bash"]
+ENTRYPOINT ["fixuid", "-q", "/ros_entrypoint.sh", "/bin/bash"]

experimental/beluga_demo_mh_amcl/config/mh_amcl_params.yaml

@@ -35,5 +35,5 @@ mh_amcl:
     # Bond timeout
     bond_timeout: 8.0
 
-    # Standard deviation for the LiDAR
-    distance_perception_error: 0.01
+    # Standard deviation for the LiDAR (shouldn't be below 0.2 for the correct computation of the qualities)
+    distance_perception_error: 0.5

…perimental/beluga_demo_mh_amcl/README.md …mental/beluga_demo_mh_amcl/doc/README.mdexperimental/beluga_demo_mh_amcl/README.md renamed to experimental/beluga_demo_mh_amcl/doc/README.md experimental/beluga_demo_mh_amcl/README.md renamed to experimental/beluga_demo_mh_amcl/doc/README.md

@@ -1,6 +1,8 @@
 # Beluga Multi-Hypotheses AMCL demo
 
-## How the algorithm work
+**NOTE**: This demo is experimental and a work in progress. It is **not** a complete proof-of-concept or a production-ready integration. Expect bugs, breaking changes, and incomplete features.
+
+## How the algorithm works
 
 Traditional AMCL is extended by managing multiple populations of particles (each population is a hypothesis). Particles are created, destroyed, or merged based on their consistency with sensory data, choosing the best fitting hypothesis in each iteration.
 
@@ -23,8 +25,8 @@ The map-matching is done with 4 levels of resolution to improve computational co
 
 ### Steps for managing the hypotheses
 
-1. Start: At the start of the robot operation, P(0,t0) (first set of particles, hypothesis) is started at the robot’s initial position, if known.
-2. Creation: Periodically, a cascade map matching algorithm is used to determine which map positions the latest sensory readings could be obtained. If a position with a high match is found (high hit field), a new P(k,t) is started at this position.
+1. Start: At the start of the robot operation, P(0,t0) (first set of particles, hypothesis) is started at the robot initial position, if known.
+2. Creation: Periodically, a cascade map matching algorithm is used to determine in which map positions the latest sensor readings could have been obtained. If a position with a high match is found (high hit field), a new P(k,t) is initialized at this position.
 3. Destruction: If Quality(P(k,t)) is less than some threshold and |Pt| > 0, then P(k,t) is considered to be wrong, and it is removed.
 4. Merge: Even if two P(i,t) and P(j,t), start at different positions, they could end up converging to the same position. In this case, they are mixed in a P(k,t) containing the particles with more weight.
 

experimental/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/mh_amcl.hpp

@@ -51,6 +51,7 @@ namespace mh_amcl
       message_filters::Subscriber<sensor_msgs::msg::LaserScan, rclcpp_lifecycle::LifecycleNode>;
   using LaserScanFilter = tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>;
 
+  // TODO (jesus): Review if the lifecycle approach is worth it or use a basic ROS 2 node
   /**
    * MH_AMCL_Node class
    *
@@ -109,6 +110,7 @@ namespace mh_amcl
     rclcpp::TimerBase::SharedPtr publish_particles_timer_;
     rclcpp::TimerBase::SharedPtr publish_position_timer_;
 
+    // TODO (jesus): This should be a struct of parameters, also merge with the 'particles_distribution' params
     // Configurable params
     bool use_sim_time_;
     int max_hypotheses_;

experimental/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/particles_distribution.hpp

@@ -161,6 +161,7 @@ namespace mh_amcl
     // Costmap and Likelihood Field Sensor model
     std::shared_ptr<beluga_ros::OccupancyGrid> costmap_;
 
+    // TODO (jesus): This should be a struct of parameters
     // MH-AMCL Parameters
     int max_particles_;
     int min_particles_;

experimental/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/reweight_and_update_hits_action.hpp

@@ -17,8 +17,8 @@
 
 #include <range/v3/functional/bind_back.hpp>
 
-#include <sensor_msgs/msg/laser_scan.hpp>
 #include <beluga_ros/occupancy_grid.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
 #include <tf2/transform_datatypes.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
@@ -57,7 +57,7 @@ namespace mh_amcl::actions
 
                     tf2::Transform laser2point = utils::get_transform_to_read(scan, static_cast<int>(j));
                     double err_m = utils::get_error_distance_to_obstacle(
-                        map2bf, base_to_laser_tf, laser2point, scan, costmap, distance_perception_error);
+                        map2bf, base_to_laser_tf, laser2point, scan, costmap);
 
                     if (!std::isinf(err_m))
                     {
@@ -73,9 +73,16 @@ namespace mh_amcl::actions
                 if (valid_beams > 0)
                 {
                     particle.weight += total_prob;
-                    // TODO: Review if hits should be computed like this
                     particle.hits = total_prob / static_cast<float>(scan.ranges.size());
                 }
+                // TODO: Review if hits should be computed like this (not having good results so far)
+                // // Update the particle's weight and hits using the average likelihood
+                // if (valid_beams > 0)
+                // {
+                //     const float average_likelihood = total_prob / static_cast<float>(valid_beams);
+                //     particle.weight *= average_likelihood;
+                //     particle.hits = average_likelihood;
+                // }
                 else
                 {
                     // Assign a small weight and 0 hits if no valid beams
@@ -104,4 +111,4 @@ namespace mh_amcl::actions
 
 } // namespace mh_amcl::actions
 
-#endif // MH_AMCL__REWEIGHT_UPDATE_HITS_ACTION_HPP_
+#endif // MH_AMCL__REWEIGHT_UPDATE_HITS_ACTION_HPP_

experimental/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/utils.hpp

@@ -122,7 +122,7 @@ namespace utils
         const tf2::Transform &map2bf, const tf2::Transform &bf2laser,
         const tf2::Transform &laser2point,
         const sensor_msgs::msg::LaserScan &scan,
-        std::shared_ptr<beluga_ros::OccupancyGrid> costmap, double o);
+        std::shared_ptr<beluga_ros::OccupancyGrid> costmap);
 
     signed char get_cost(const tf2::Transform &transform,
                          std::shared_ptr<beluga_ros::OccupancyGrid> costmap);

experimental/beluga_demo_mh_amcl/src/mh_amcl/utils.cpp

@@ -239,8 +239,7 @@ namespace utils
   double get_error_distance_to_obstacle(
       const tf2::Transform &map2bf, const tf2::Transform &bf2laser,
       const tf2::Transform &laser2point, const sensor_msgs::msg::LaserScan &scan,
-      std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
-      double distance_perception_error)
+      std::shared_ptr<beluga_ros::OccupancyGrid> costmap)
   {
     if (std::isinf(laser2point.getOrigin().x()) ||
         std::isnan(laser2point.getOrigin().x()))
@@ -261,7 +260,9 @@ namespace utils
     }
 
     float dist = costmap->resolution();
-    while (dist < (3.0 * distance_perception_error))
+    // TODO (jesus): Review value of this constant and how it affects performance and result of the filter
+    const float max_obstacle_search_distance = 0.3;
+    while (dist < max_obstacle_search_distance)
     {
       uvector.setOrigin(unit * dist);
       // For positive