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,82 @@ 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.;
+    auto x_offset = pose.x - map_metadata_.origin.position.x;
+    auto y_offset = pose.y - map_metadata_.origin.position.y;
+    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) {
+        auto x = point.first * std::cos(pose.theta) + point.second * std::sin(pose.theta);
+        auto y = point.first * std::sin(pose.theta) + point.second * std::cos(pose.theta);
+        x = (x + x_offset) / map_metadata_.resolution;
+        y = (y + y_offset) / map_metadata_.resolution;
+        auto x_index = static_cast<std::intmax_t>(std::floor(x));
+        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);
+    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 & laser_transform)
+  {
+    auto base_transform = tf2::Transform{};
+    tf2::convert(laser_transform.transform, base_transform);
+    auto range_min = std::max(laser_scan.range_min, static_cast<float>(params_.min_laser_distance));
+    auto 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) {
+      auto range = laser_scan.ranges[index];
+      if (std::isnan(range) || range < range_min || range > range_max) {
+        continue;
+      }
+      // Store points in the robot's reference frame
+      using ScalarType = decltype(laser_scan.angle_increment);
+      auto angle = laser_scan.angle_min + static_cast<ScalarType>(index) *
+        laser_scan.angle_increment;
+      auto point = base_transform * tf2::Vector3{
+        range * std::cos(angle),
+        range * std::sin(angle),
+        0.0};
+      point_buffer.emplace_back(point.x(), point.y());
+    }
+    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,10 +204,10 @@ 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_;
+  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)
   {
@@ -203,15 +274,6 @@ class LikelihoodSensorModel : public Mixin
     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>

beluga_amcl/src/amcl_node.cpp

@@ -14,12 +14,18 @@
 
 #include <beluga_amcl/amcl_node.hpp>
 
+#include <tf2/convert.h>
+#include <tf2/utils.h>
+#include <tf2_ros/create_timer_ros.h>
+
 #include <chrono>
 #include <limits>
 #include <memory>
 
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <lifecycle_msgs/msg/state.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace beluga_amcl
 {
@@ -36,13 +42,32 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options)
     declare_parameter("global_frame_id", rclcpp::ParameterValue("map"), descriptor);
   }
 
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description = "The name of the coordinate frame to use for odometry.";
+    declare_parameter("odom_frame_id", rclcpp::ParameterValue("odom"), descriptor);
+  }
+
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description = "The name of the coordinate frame to use for the robot base.";
+    declare_parameter("base_frame_id", rclcpp::ParameterValue("base_footprint"), descriptor);
+  }
+
   {
     auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
     descriptor.description =
       "Topic to subscribe to in order to receive the map to localize on.";
     declare_parameter("map_topic", rclcpp::ParameterValue("map"), descriptor);
   }
 
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description =
+      "Topic to subscribe to in order to receive the laser scan for localization.";
+    declare_parameter("scan_topic", rclcpp::ParameterValue("scan"), descriptor);
+  }
+
   {
     auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
     descriptor.description = "Minimum allowed number of particles.";
@@ -264,6 +289,27 @@ AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State &)
 
   RCLCPP_INFO(get_logger(), "Subscribed to map_topic: %s", map_sub_->get_topic_name());
 
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
+  tf_buffer_->setCreateTimerInterface(
+    std::make_shared<tf2_ros::CreateTimerROS>(
+      get_node_base_interface(),
+      get_node_timers_interface()));
+  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(shared_from_this());
+  tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
+
+  laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+      rclcpp_lifecycle::LifecycleNode>>(
+    shared_from_this(), get_parameter("scan_topic").as_string(), rmw_qos_profile_sensor_data);
+
+  laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
+    *laser_scan_sub_, *tf_buffer_, get_parameter("base_frame_id").as_string(), 50,
+    get_node_logging_interface(),
+    get_node_clock_interface(),
+    tf2::durationFromSec(1.0));
+
+  laser_scan_connection_ = laser_scan_filter_->registerCallback(
+    std::bind(&AmclNode::laser_callback, this, std::placeholders::_1));
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -272,9 +318,12 @@ AmclNode::CallbackReturn AmclNode::on_deactivate(const rclcpp_lifecycle::State &
   RCLCPP_INFO(get_logger(), "Deactivating");
   particle_cloud_pub_->on_deactivate();
   likelihood_field_pub_->on_deactivate();
+  laser_scan_sub_.reset();
   map_sub_.reset();
   bond_.reset();
-
+  tf_listener_.reset();
+  tf_broadcaster_.reset();
+  tf_buffer_.reset();
   return CallbackReturn::SUCCESS;
 }
 
@@ -309,6 +358,7 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map)
 
   auto likelihood_model_params = LikelihoodSensorModelParam{
     get_parameter("laser_likelihood_max_dist").as_double(),
+    get_parameter("laser_min_range").as_double(),
     get_parameter("laser_max_range").as_double(),
     get_parameter("z_hit").as_double(),
     get_parameter("z_rand").as_double(),
@@ -353,6 +403,26 @@ void AmclNode::timer_callback()
   }
 }
 
+void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
+{
+  if (!particle_filter_) {
+    return;
+  }
+
+  try {
+    using namespace std::chrono_literals;
+    auto transform_stamped = tf_buffer_->lookupTransform(
+      get_parameter("base_frame_id").as_string(),
+      laser_scan->header.frame_id,
+      laser_scan->header.stamp,
+      0s);
+    particle_filter_->update_sensor(*laser_scan, transform_stamped);
+    particle_filter_->update();
+  } catch (const tf2::TransformException & error) {
+    RCLCPP_ERROR(get_logger(), "Could not transform laser: %s", error.what());
+  }
+}
+
 }  // namespace beluga_amcl
 
 RCLCPP_COMPONENTS_REGISTER_NODE(beluga_amcl::AmclNode)