Draft: Publish likelihood_data

Signed-off-by: Diego Palma <dpalma@symbotic.com>

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>
 
@@ -89,7 +90,7 @@ class BaseAMCLNode : public rclcpp_lifecycle::LifecycleNode {
   void autostart_callback();
 
   /// User provided extra steps for the autostart process.
-  virtual void do_autostart_callback(){};
+  virtual void do_autostart_callback() {};
 
   // Configuration points for extra steps needed for the different AMCL variants.
 
@@ -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>
@@ -122,7 +123,7 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", ""
 
   {
     auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
-    descriptor.description = "If false, Likelihood Field sensor model won't model unknown space.";
+    descriptor.description = "Whether to model unknown space or assume it free.";
     declare_parameter("model_unknown_space", false, descriptor);
   }
 
@@ -418,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/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_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

@@ -62,7 +62,7 @@ amcl:
     # 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 : false
+    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,50 @@
+// 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 {
+
+inline void assign_likelihood_field(
+    const beluga::ValueGrid2<float>& likelihood_field,
+    nav_msgs::msg::OccupancyGrid& message) {
+  // 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) {
+    // Convert T to int8_t if necessary
+    message.data[i] = static_cast<int8_t>(grid_data[i]);
+  }
+}
+
+}  // 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_)) {