Implement likelihood estimation for particles

beluga_amcl/CMakeLists.txt

@@ -13,6 +13,8 @@ find_package(nav2_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 add_executable(amcl_node)
 target_sources(amcl_node PUBLIC src/main.cpp src/amcl_node.cpp)
@@ -26,7 +28,9 @@ ament_target_dependencies(amcl_node PUBLIC
   nav2_msgs
   rclcpp
   rclcpp_components
-  rclcpp_lifecycle)
+  rclcpp_lifecycle
+  sensor_msgs
+  tf2_geometry_msgs)
 
 install(TARGETS amcl_node
   DESTINATION lib/${PROJECT_NAME})

beluga_amcl/include/beluga_amcl/amcl_node.hpp

@@ -15,6 +15,12 @@
 #ifndef BELUGA_AMCL__AMCL_NODE_HPP_
 #define BELUGA_AMCL__AMCL_NODE_HPP_
 
+#include <message_filters/subscriber.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
 #include <beluga/algorithm/particle_filter.h>
 
 #include <memory>
@@ -24,6 +30,7 @@
 #include <nav2_msgs/msg/particle_cloud.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
 
 namespace beluga_amcl
 {
@@ -32,7 +39,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
 {
 public:
   using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-  using ParticleFilter = beluga::AMCL<MockMotionModel, LikelihoodSensorModel, Pose>;
+  using ParticleFilter = beluga::AMCL<StationaryMotionModel, LikelihoodSensorModel, Pose>;
 
   explicit AmclNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   virtual ~AmclNode();
@@ -46,6 +53,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
 
   void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr);
   void timer_callback();
+  void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr);
 
   std::unique_ptr<ParticleFilter> particle_filter_;
   std::unique_ptr<bond::Bond> bond_;
@@ -56,6 +64,14 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr
     likelihood_field_pub_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
+  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+    rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
+  message_filters::Connection laser_scan_connection_;
 };
 
 }  // namespace beluga_amcl

beluga_amcl/include/beluga_amcl/models.hpp

@@ -23,6 +23,9 @@
 #include <beluga/algorithm/distance_map.h>
 #include <beluga/spatial_hash.h>
 
+#include <tf2/convert.h>
+#include <tf2/utils.h>
+
 #include <algorithm>
 #include <cmath>
 #include <queue>
@@ -32,8 +35,10 @@
 #include <vector>
 
 #include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <range/v3/view.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
 
 namespace beluga_amcl
 {
@@ -62,19 +67,37 @@ struct Pose
 
 // TODO(nahuel): Replace this with a real motion model.
 template<class Mixin>
-class MockMotionModel : public Mixin
+class StationaryMotionModel : public Mixin
 {
 public:
   template<class ... Args>
-  explicit MockMotionModel(Args && ... args)
+  explicit StationaryMotionModel(Args && ... args)
   : Mixin(std::forward<Args>(args)...) {}
 
-  [[nodiscard]] auto apply_motion(const Pose & state) {return state;}
+  [[nodiscard]] auto apply_motion(const Pose & state) const
+  {
+    static thread_local std::mt19937 generator{std::random_device()()};
+    auto distribution = std::normal_distribution<>{0, 0.02};
+    auto pose = state;
+    pose.x += distribution(generator);
+    pose.y += distribution(generator);
+    pose.theta = wrap_angle(pose.theta + distribution(generator));
+    return pose;
+  }
+
+private:
+  static double wrap_angle(double angle)
+  {
+    // Wrap angle between -pi and pi
+    angle = std::fmod(angle + M_PI, 2.0 * M_PI);
+    return angle <= 0.0 ? angle + M_PI : angle - M_PI;
+  }
 };
 
 struct LikelihoodSensorModelParam
 {
   double max_obstacle_distance;
+  double min_laser_distance;
   double max_laser_distance;
   double z_hit;
   double z_random;
@@ -95,34 +118,84 @@ class LikelihoodSensorModel : public Mixin
     params_{params},
     map_metadata_{map.info},
     free_cells_{make_free_cells_list(map)},
-    index_distribution_{0, free_cells_.size() - 1},
     likelihood_field_{make_likelihood_field(params, map)}
   {}
 
-  [[nodiscard]] double importance_weight(const Pose &)
+  [[nodiscard]] double importance_weight(const Pose & pose) const
   {
-    // TODO(nahuel): Implement likelihood estimation.
-    return 1.;
+    // Compute offset in scaled map coordinates
+    const auto x_offset = (pose.x - map_metadata_.origin.position.x) / map_metadata_.resolution;
+    const auto y_offset = (pose.y - map_metadata_.origin.position.y) / map_metadata_.resolution;
+    const auto lock = std::shared_lock<std::shared_mutex>(points_mutex_);
+    /* *INDENT-OFF* Avoid uncrustify reformat */
+    return std::transform_reduce(
+      points_.cbegin(), points_.cend(), 0.0, std::plus{},
+      [this, pose, x_offset, y_offset](const auto & point) {
+        const auto x = point.first * std::cos(pose.theta) - point.second * std::sin(pose.theta) + x_offset;
+        const auto y = point.first * std::sin(pose.theta) + point.second * std::cos(pose.theta) + y_offset;
+        const auto x_index = static_cast<std::intmax_t>(std::floor(x));
+        const auto y_index = static_cast<std::intmax_t>(std::floor(y));
+        if (x_index >= map_metadata_.width || y_index >= map_metadata_.height) {
+          return 0.;
+        }
+        return likelihood_field_[x_index + y_index * map_metadata_.width];
+      });
+    /* *INDENT-ON* */
   }
 
   template<class Generator>
-  [[nodiscard]] auto generate_random_state(Generator & generator)
+  [[nodiscard]] auto generate_random_state(Generator & generator) const
   {
+    auto index_distribution = std::uniform_int_distribution<std::size_t>{0, free_cells_.size() - 1};
+    auto theta_distribution = std::uniform_real_distribution<double>{-M_PI, M_PI};
     auto pose = Pose{};
-    auto [x, y] = xy_from_index(map_metadata_, free_cells_[index_distribution_(generator)]);
-    pose.x = x;
-    pose.y = y;
-    pose.theta = theta_distribution_(generator);
+    const auto index = free_cells_[index_distribution(generator)];
+    pose.x = static_cast<double>(index % map_metadata_.width) + 0.5;
+    pose.y = static_cast<double>(index / map_metadata_.width) + 0.5;
+    pose.x = pose.x * map_metadata_.resolution + map_metadata_.origin.position.x;
+    pose.y = pose.y * map_metadata_.resolution + map_metadata_.origin.position.y;
+    pose.theta = theta_distribution(generator);
     return pose;
   }
 
+  void update_sensor(
+    const sensor_msgs::msg::LaserScan & laser_scan,
+    const geometry_msgs::msg::TransformStamped & base_to_laser_stamped)
+  {
+    auto base_to_laser_transform = tf2::Transform{};
+    tf2::convert(base_to_laser_stamped.transform, base_to_laser_transform);
+    const float range_min =
+      std::max(laser_scan.range_min, static_cast<float>(params_.min_laser_distance));
+    const float range_max =
+      std::min(laser_scan.range_max, static_cast<float>(params_.max_laser_distance));
+    auto point_buffer = decltype(points_) {};
+    point_buffer.reserve(laser_scan.ranges.size());
+    for (std::size_t index = 0; index < laser_scan.ranges.size(); ++index) {
+      float range = laser_scan.ranges[index];
+      if (std::isnan(range) || range < range_min || range > range_max) {
+        continue;
+      }
+      // Store points in the robot's reference frame with scaled coordinates
+      const float angle = laser_scan.angle_min +
+        static_cast<float>(index) * laser_scan.angle_increment;
+      const auto point = base_to_laser_transform * tf2::Vector3{
+        range * std::cos(angle),
+        range * std::sin(angle),
+        0.0};
+      point_buffer.emplace_back(
+        point.x() / map_metadata_.resolution,
+        point.y() / map_metadata_.resolution);
+    }
+    const auto lock = std::lock_guard<std::shared_mutex>(points_mutex_);
+    points_ = std::move(point_buffer);
+  }
+
   auto get_likelihood_field_as_gridmap() const
   {
+    const std::size_t size = map_metadata_.width * map_metadata_.height;
     auto message = nav_msgs::msg::OccupancyGrid{};
     message.info = map_metadata_;
-    const std::size_t size = map_metadata_.width * map_metadata_.height;
     message.data.resize(size);
-
     for (std::size_t index = 0; index < size; ++index) {
       message.data[index] = static_cast<std::int8_t>(likelihood_field_[index] * 100);
     }
@@ -133,11 +206,12 @@ class LikelihoodSensorModel : public Mixin
   LikelihoodSensorModelParam params_;
   nav_msgs::msg::MapMetaData map_metadata_;
   std::vector<std::size_t> free_cells_;
-  std::uniform_int_distribution<std::size_t> index_distribution_;
-  std::uniform_real_distribution<double> theta_distribution_{-M_PI, M_PI};
-
   std::vector<double> likelihood_field_;
 
+  // TODO(nahuel): Create a mixin type to ensure thread safety for sensor and motion models.
+  std::vector<std::pair<double, double>> points_;
+  mutable std::shared_mutex points_mutex_;
+
   static std::vector<std::size_t> make_free_cells_list(const nav_msgs::msg::OccupancyGrid & map)
   {
     const std::size_t size = map.info.width * map.info.height;
@@ -167,9 +241,9 @@ class LikelihoodSensorModel : public Mixin
         squared_resolution = map.info.resolution * map.info.resolution,
         squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance
       ](std::size_t first, std::size_t second) {
-        auto delta_x =
+        const auto delta_x =
           static_cast<std::intmax_t>(first % width) - static_cast<std::intmax_t>(second % width);
-        auto delta_y =
+        const auto delta_y =
           static_cast<std::intmax_t>(first / width) - static_cast<std::intmax_t>(second / width);
         return std::min(
           static_cast<double>(delta_x * delta_x + delta_y * delta_y) * squared_resolution,
@@ -200,18 +274,10 @@ class LikelihoodSensorModel : public Mixin
         return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset;
       };
 
-    auto distance_map = nearest_obstacle_distance_map(obstacle_map, squared_distance, neighbors);
+    const auto distance_map = nearest_obstacle_distance_map(
+      obstacle_map, squared_distance, neighbors);
     return distance_map | ranges::views::transform(to_likelihood) | ranges::to<std::vector>;
   }
-
-  static auto xy_from_index(
-    const nav_msgs::msg::MapMetaData & info,
-    std::size_t index)
-  {
-    return std::make_pair(
-      (static_cast<double>(index % info.width) + 0.5) * info.resolution + info.origin.position.x,
-      (static_cast<double>(index / info.width) + 0.5) * info.resolution + info.origin.position.y);
-  }
 };
 
 }  // namespace beluga_amcl

beluga_amcl/package.xml

@@ -20,6 +20,10 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>