Expose model_unknown_space parameter to ROS2

beluga_amcl/config/Amcl.cfg

@@ -263,6 +263,12 @@ gen.add(
     default=0.05, min=0., max=1.
 )
 
+gen.add(
+    "model_unknown_space", bool_t, 0,
+    "Whether to model unknown space or assume it free.",
+    default=False,
+)
+
 gen.add(
     "laser_z_max", double_t, 0,
     "Mixture weight for the probability of getting max range measurements.",

beluga_amcl/include/beluga_amcl/ros2_common.hpp

@@ -24,6 +24,7 @@
 
 #include <bondcpp/bond.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/time.hpp>
 
@@ -118,6 +119,8 @@ class BaseAMCLNode : public rclcpp_lifecycle::LifecycleNode {
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr particle_markers_pub_;
   /// Estimated pose publisher.
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+  /// Likelihood field publisher
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr likelihood_field_pub_;
   /// Node bond with the lifecycle manager.
   std::unique_ptr<bond::Bond> bond_;
   /// Timer for periodic particle cloud updates.

beluga_amcl/src/amcl_node.cpp

@@ -63,6 +63,7 @@
 #include <beluga/sensor/beam_model.hpp>
 #include <beluga/sensor/likelihood_field_model.hpp>
 #include <beluga_ros/amcl.hpp>
+#include <beluga_ros/likelihood_field.hpp>
 #include <beluga_ros/messages.hpp>
 #include <beluga_ros/particle_cloud.hpp>
 #include <beluga_ros/tf2_sophus.hpp>
@@ -120,6 +121,12 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", ""
     declare_parameter("z_rand", rclcpp::ParameterValue(0.5), descriptor);
   }
 
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description = "Whether to model unknown space or assume it free.";
+    declare_parameter("model_unknown_space", false, descriptor);
+  }
+
   {
     auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
     descriptor.description = "Mixture weight for the probability of getting max range measurements.";
@@ -304,6 +311,7 @@ auto AmclNode::get_sensor_model(std::string_view name, nav_msgs::msg::OccupancyG
     params.z_hit = get_parameter("z_hit").as_double();
     params.z_random = get_parameter("z_rand").as_double();
     params.sigma_hit = get_parameter("sigma_hit").as_double();
+    params.model_unknown_space = get_parameter("model_unknown_space").as_bool();
     return beluga::LikelihoodFieldModel{params, beluga_ros::OccupancyGrid{map}};
   }
   if (name == kBeamSensorModelName) {
@@ -411,6 +419,13 @@ void AmclNode::do_periodic_timer_callback() {
     beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
     particle_markers_pub_->publish(message);
   }
+
+  if (likelihood_field_pub_->get_subscription_count() > 0) {
+    auto message = beluga_ros::msg::OccupancyGrid{};
+    beluga_ros::assign_likelihood_field(particle_filter_->get_likelihood_field(), message);
+    beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
+    likelihood_field_pub_->publish(message);
+  }
 }
 
 void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) {

beluga_amcl/src/amcl_nodelet.cpp

@@ -194,6 +194,7 @@ auto AmclNodelet::get_sensor_model(std::string_view name, const nav_msgs::Occupa
     params.z_hit = config_.laser_z_hit;
     params.z_random = config_.laser_z_rand;
     params.sigma_hit = config_.laser_sigma_hit;
+    params.model_unknown_space = config_.model_unknown_space;
     return beluga::LikelihoodFieldModel{params, beluga_ros::OccupancyGrid{map}};
   }
   if (name == kBeamSensorModelName) {

beluga_amcl/src/ros2_common.cpp

@@ -408,6 +408,8 @@ BaseAMCLNode::CallbackReturn BaseAMCLNode::on_configure(const rclcpp_lifecycle::
   particle_markers_pub_ =
       create_publisher<visualization_msgs::msg::MarkerArray>("particle_markers", rclcpp::SystemDefaultsQoS());
   pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", rclcpp::SystemDefaultsQoS());
+  likelihood_field_pub_ =
+      create_publisher<nav_msgs::msg::OccupancyGrid>("likelihood_field", rclcpp::SystemDefaultsQoS());
   do_configure(state);
   return CallbackReturn::SUCCESS;
 };
@@ -417,6 +419,7 @@ BaseAMCLNode::CallbackReturn BaseAMCLNode::on_deactivate(const rclcpp_lifecycle:
   particle_cloud_pub_->on_deactivate();
   particle_markers_pub_->on_deactivate();
   pose_pub_->on_deactivate();
+  likelihood_field_pub_->on_deactivate();
   initial_pose_sub_.reset();
   tf_listener_.reset();
   tf_broadcaster_.reset();
@@ -451,6 +454,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseAM
   particle_cloud_pub_->on_activate();
   particle_markers_pub_->on_activate();
   pose_pub_->on_activate();
+  likelihood_field_pub_->on_activate();
   {
     initial_pose_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
         get_parameter("initial_pose_topic").as_string(), rclcpp::SystemDefaultsQoS(),

beluga_amcl/test/test_amcl_node.cpp

@@ -733,6 +733,8 @@ INSTANTIATE_TEST_SUITE_P(
         rclcpp::Parameter("z_hit", 2.0),
         rclcpp::Parameter("z_rand", -1.0),
         rclcpp::Parameter("z_rand", 2.0),
+        rclcpp::Parameter("model_unknown_space", false),
+        rclcpp::Parameter("model_unknown_space", true),
         rclcpp::Parameter("z_max", -1.0),
         rclcpp::Parameter("z_max", 2.0),
         rclcpp::Parameter("z_short", -1.0),

beluga_example/params/default.ros.yaml

@@ -63,6 +63,8 @@ laser_max_beams: 100
 laser_z_hit: 0.5
 # Weight used to combine the probability of random noise in perception.
 laser_z_rand: 0.5
+# Whether to model unknown space or assume it free.
+model_unknown_space: false
 # Weight used to combine the probability of getting short readings.
 laser_z_short: 0.05
 # Weight used to combine the probability of getting max range readings.

beluga_example/params/default.ros2.yaml

@@ -61,6 +61,8 @@ amcl:
     z_hit: 0.5
     # Weight used to combine the probability of random noise in perception.
     z_rand: 0.5
+    # Whether to model unknown space or assume it free.
+    model_unknown_space: true
     # Weight used to combine the probability of getting short readings.
     z_short: 0.05
     # Weight used to combine the probability of getting max range readings.

beluga_ros/include/beluga_ros/amcl.hpp

@@ -31,6 +31,7 @@
 #include <beluga/policies.hpp>
 #include <beluga/random.hpp>
 #include <beluga/sensor.hpp>
+#include <beluga/sensor/data/value_grid.hpp>
 #include <beluga/views/sample.hpp>
 
 #include <beluga_ros/laser_scan.hpp>
@@ -133,6 +134,9 @@ class Amcl {
   /// Returns a reference to the current set of particles.
   [[nodiscard]] const auto& particles() const { return particles_; }
 
+  /// Returns a reference to the current likelihood field.
+  [[nodiscard]] const auto& get_likelihood_field() const { return likelihood_field_; }
+
   /// Initialize particles using a custom distribution.
   template <class Distribution>
   void initialize(Distribution distribution) {
@@ -178,6 +182,7 @@ class Amcl {
 
  private:
   beluga::TupleVector<particle_type> particles_;
+  beluga::ValueGrid2<float> likelihood_field_ = createCleanValueGrid2<float>(0, 2, 2);  // Dummy initialization
 
   AmclParams params_;
   beluga::MultivariateUniformDistribution<Sophus::SE2d, beluga_ros::OccupancyGrid> map_distribution_;
@@ -193,6 +198,17 @@ class Amcl {
   beluga::RollingWindow<Sophus::SE2d, 2> control_action_window_;
 
   bool force_update_{true};
+
+  // Created to initialize likelihood_field_
+  template <typename T>
+  beluga::ValueGrid2<T>
+  createCleanValueGrid2(T default_value, std::size_t width, std::size_t height, double resolution = 1.0) {
+    // Create a data vector with default_value for each cell
+    std::vector<T> data(width * height, default_value);
+
+    // Construct and return the ValueGrid2 object
+    return beluga::ValueGrid2<T>(std::move(data), width, resolution);
+  }
 };
 
 }  // namespace beluga_ros

beluga_ros/include/beluga_ros/likelihood_field.hpp

@@ -0,0 +1,58 @@
+// Copyright 2024 Ekumen, Inc.
+//
+// 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 BELUGA_ROS_LIKELIHOOD_FIELD_HPP
+#define BELUGA_ROS_LIKELIHOOD_FIELD_HPP
+
+#include <cstdint>
+
+#include <beluga/sensor/data/value_grid.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+/**
+ * \file
+ * \brief Utilities for likelihood data over ROS interfaces.
+ */
+
+namespace beluga_ros {
+
+/// Convert from likelihood data to ROS2 message.
+inline void assign_likelihood_field(
+    const beluga::ValueGrid2<float>& likelihood_field,
+    nav_msgs::msg::OccupancyGrid& message,
+    double free_threshold = 0.2,
+    double occupied_threshold = 0.25) {
+  // Set metadata
+  message.info.width = static_cast<unsigned int>(likelihood_field.width());
+  message.info.height = static_cast<unsigned int>(likelihood_field.height());
+  message.info.resolution = static_cast<float>(likelihood_field.resolution());
+
+  // Populate the data field with the grid data
+  message.data.resize(likelihood_field.size());
+  const auto& grid_data = likelihood_field.data();
+
+  for (std::size_t i = 0; i < likelihood_field.size(); ++i) {
+    if (grid_data[i] <= free_threshold) {
+      message.data[i] = 0;  // Free
+    } else if (grid_data[i] >= occupied_threshold) {
+      message.data[i] = 100;  // Occupied
+    } else {
+      message.data[i] = -1;  // Unknown
+    }
+  }
+}
+
+}  // namespace beluga_ros
+
+#endif  // BELUGA_ROS_LIKELIHOOD_FIELD_HPP

beluga_ros/src/amcl.cpp

@@ -76,6 +76,11 @@ auto Amcl::update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_sc
       },
       execution_policy_, motion_model_, sensor_model_);
 
+  if (std::holds_alternative<beluga::LikelihoodFieldModel<beluga_ros::OccupancyGrid>>(sensor_model_)) {
+    likelihood_field_ =
+        std::get<beluga::LikelihoodFieldModel<beluga_ros::OccupancyGrid>>(sensor_model_).likelihood_field();
+  }
+
   const double random_state_probability = random_probability_estimator_(particles_);
 
   if (resample_policy_(particles_)) {