Restore particle cloud visualization using markers (#377)

### Proposed changes Follow-up to #372. As the title reads, this restores the functionality we had with `nav2_msgs/ParticleCloud` using `visualization_msgs/MarkerArray` instead. https://github.com/Ekumen-OS/beluga/assets/13500507/9b4e84b8-0b27-40cd-a052-63f1ef91dadb #### Type of change - [ ] 🐛 Bugfix (change which fixes an issue) - [x] 🚀 Feature (change which adds functionality) - [ ] 📚 Documentation (change which fixes or extends documentation) ### Checklist - [x] Lint and unit tests (if any) pass locally with my changes - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added necessary documentation (if appropriate) - [x] All commits have been signed for [DCO](https://developercertificate.org/) --------- Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
Ekumen-OS · May 21, 2024 · 33c83f7 · 33c83f7
1 parent bda970b
commit 33c83f7
Show file tree

Hide file tree

Showing 16 changed files with 406 additions and 19 deletions.
diff --git a/beluga/include/beluga/algorithm/spatial_hash.hpp b/beluga/include/beluga/algorithm/spatial_hash.hpp
@@ -146,9 +146,8 @@ class spatial_hash<Tuple<Types...>, std::enable_if_t<(std::is_arithmetic_v<Types
 template <>
 class spatial_hash<Sophus::SE2d, void> {
  public:
-  /// Constructs a spatial hasher from an `std::array` of doubles.
+  /// Constructs a spatial hasher given per-coordinate resolutions.
   /**
-   *
    * \param x_clustering_resolution Clustering resolution for the X axis, in meters.
    * \param y_clustering_resolution Clustering resolution for the Y axis, in meters.
    * \param theta_clustering_resolution Clustering resolution for the Theta axis, in radians.
@@ -159,6 +158,14 @@ class spatial_hash<Sophus::SE2d, void> {
       double theta_clustering_resolution)
       : underlying_hasher_{{x_clustering_resolution, y_clustering_resolution, theta_clustering_resolution}} {};
 
+  /// Constructs a spatial hasher given per-group resolutions.
+  /**
+   * \param linear_clustering_resolution Clustering resolution for translational coordinates, in meters.
+   * \param angular_clustering_resolution Clustering resolution for rotational coordinates, in radians.
+   */
+  explicit spatial_hash(double linear_clustering_resolution, double angular_clustering_resolution)
+      : spatial_hash(linear_clustering_resolution, linear_clustering_resolution, angular_clustering_resolution){};
+
   /// Default constructor
   spatial_hash() = default;
 

diff --git a/beluga_amcl/docs/ros2-reference.md b/beluga_amcl/docs/ros2-reference.md
@@ -205,6 +205,9 @@ Also available as a standalone `amcl_node` executable.
 `particle_cloud`
 : Estimated pose distribution published as `geometry_msgs/msg/PoseArray` messages, using a sensor data QoS policy. It will only be published if subscribers are found.
 
+`particle_markers`
+: Estimated pose distribution visualization published as `visualization_msgs/msg/MarkerArray` messages, using a system default QoS policy. Each particle is depicted using an arrow. Each arrow is colored and scaled according to the weight of the corresponding state in the distribution. Large, bright red arrows represent the most likely states, whereas small, dim purple arrows represent the least likely states. The rest lie in between. It will only be published if subscribers are found.
+
 `pose`
 : Mean and covariance of the estimated pose distribution published as `geometry_msgs/msg/PoseWithCovarianceStamped` messages (assumed Gaussian), using a system default QoS policy.
 

diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp
@@ -32,6 +32,7 @@
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <std_srvs/srv/empty.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <beluga/beluga.hpp>
 #include <beluga_ros/amcl.hpp>
@@ -143,6 +144,9 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode {
 
   /// Particle cloud publisher.
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr particle_cloud_pub_;
+  /// Particle markers publisher.
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr particle_markers_pub_;
+
   /// Estimated pose publisher.
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
 

diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp
@@ -177,13 +177,16 @@ void AmclNode::autostart_callback() {
 AmclNode::CallbackReturn AmclNode::on_configure(const rclcpp_lifecycle::State&) {
   RCLCPP_INFO(get_logger(), "Configuring");
   particle_cloud_pub_ = create_publisher<geometry_msgs::msg::PoseArray>("particle_cloud", rclcpp::SensorDataQoS());
+  particle_markers_pub_ =
+      create_publisher<visualization_msgs::msg::MarkerArray>("particle_markers", rclcpp::SystemDefaultsQoS());
   pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", rclcpp::SystemDefaultsQoS());
   return CallbackReturn::SUCCESS;
 }
 
 AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State&) {
   RCLCPP_INFO(get_logger(), "Activating");
   particle_cloud_pub_->on_activate();
+  particle_markers_pub_->on_activate();
   pose_pub_->on_activate();
 
   {
@@ -275,6 +278,7 @@ AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State&) {
 AmclNode::CallbackReturn AmclNode::on_deactivate(const rclcpp_lifecycle::State&) {
   RCLCPP_INFO(get_logger(), "Deactivating");
   particle_cloud_pub_->on_deactivate();
+  particle_markers_pub_->on_deactivate();
   pose_pub_->on_deactivate();
   map_sub_.reset();
   initial_pose_sub_.reset();
@@ -292,6 +296,7 @@ AmclNode::CallbackReturn AmclNode::on_deactivate(const rclcpp_lifecycle::State&)
 AmclNode::CallbackReturn AmclNode::on_cleanup(const rclcpp_lifecycle::State&) {
   RCLCPP_INFO(get_logger(), "Cleaning up");
   particle_cloud_pub_.reset();
+  particle_markers_pub_.reset();
   pose_pub_.reset();
   particle_filter_.reset();
   enable_tf_broadcast_ = false;
@@ -474,14 +479,19 @@ void AmclNode::timer_callback() {
     return;
   }
 
-  if (particle_cloud_pub_->get_subscription_count() == 0) {
-    return;
+  if (particle_cloud_pub_->get_subscription_count() > 0) {
+    auto message = beluga_ros::msg::PoseArray{};
+    beluga_ros::assign_particle_cloud(particle_filter_->particles(), message);
+    beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
+    particle_cloud_pub_->publish(message);
   }
 
-  auto message = beluga_ros::msg::PoseArray{};
-  beluga_ros::assign_particle_cloud(particle_filter_->particles(), message);
-  beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
-  particle_cloud_pub_->publish(message);
+  if (particle_markers_pub_->get_subscription_count() > 0) {
+    auto message = beluga_ros::msg::MarkerArray{};
+    beluga_ros::assign_particle_cloud(particle_filter_->particles(), message);
+    beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
+    particle_markers_pub_->publish(message);
+  }
 }
 
 void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) {

diff --git a/beluga_amcl/test/test_amcl_node.cpp b/beluga_amcl/test/test_amcl_node.cpp
@@ -71,6 +71,12 @@ class BaseNodeFixture : public T {
         [this] { return tester_node_->latest_particle_cloud().has_value(); }, 1000ms, amcl_node_, tester_node_);
   }
 
+  bool wait_for_particle_markers() {
+    tester_node_->create_particle_markers_subscriber();
+    return spin_until(
+        [this] { return tester_node_->latest_particle_markers().has_value(); }, 1000ms, amcl_node_, tester_node_);
+  }
+
   bool request_global_localization() {
     if (!tester_node_->wait_for_global_localization_service(500ms)) {
       return false;
@@ -617,10 +623,18 @@ TEST_F(TestNode, IsPublishingParticleCloud) {
   amcl_node_->activate();
   tester_node_->publish_map();
   ASSERT_TRUE(wait_for_initialization());
-  tester_node_->create_particle_cloud_subscriber();
   ASSERT_TRUE(wait_for_particle_cloud());
 }
 
+TEST_F(TestNode, IsPublishingParticleMarkers) {
+  amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true});
+  amcl_node_->configure();
+  amcl_node_->activate();
+  tester_node_->publish_map();
+  ASSERT_TRUE(wait_for_initialization());
+  ASSERT_TRUE(wait_for_particle_markers());
+}
+
 TEST_F(TestNode, LaserScanWithNoOdomToBase) {
   amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", true});
   amcl_node_->configure();

diff --git a/beluga_amcl/test/test_utils/node_testing.hpp b/beluga_amcl/test/test_utils/node_testing.hpp
@@ -86,6 +86,18 @@ class TesterNode : public rclcpp::Node {
 
   const auto& latest_particle_cloud() const { return latest_particle_cloud_; }
 
+  void create_particle_markers_subscriber() {
+    particle_markers_subscriber_ = create_subscription<visualization_msgs::msg::MarkerArray>(
+        "particle_markers", rclcpp::SystemDefaultsQoS(),
+        std::bind(&TesterNode::particle_markers_callback, this, std::placeholders::_1));
+  }
+
+  void particle_markers_callback(visualization_msgs::msg::MarkerArray::SharedPtr message) {
+    latest_particle_markers_ = *message;
+  }
+
+  const auto& latest_particle_markers() const { return latest_particle_markers_; }
+
   static auto make_dummy_map() {
     auto map = nav_msgs::msg::OccupancyGrid{};
     map.header.frame_id = "map";
@@ -235,9 +247,11 @@ class TesterNode : public rclcpp::Node {
 
   SubscriberPtr<geometry_msgs::msg::PoseWithCovarianceStamped> pose_subscriber_;
   SubscriberPtr<geometry_msgs::msg::PoseArray> particle_cloud_subscriber_;
+  SubscriberPtr<visualization_msgs::msg::MarkerArray> particle_markers_subscriber_;
 
   std::optional<geometry_msgs::msg::PoseWithCovarianceStamped> latest_pose_;
   std::optional<geometry_msgs::msg::PoseArray> latest_particle_cloud_;
+  std::optional<visualization_msgs::msg::MarkerArray> latest_particle_markers_;
 
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

diff --git a/beluga_ros/cmake/ament.cmake b/beluga_ros/cmake/ament.cmake
@@ -18,9 +18,11 @@ find_package(beluga REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 ament_python_install_package(
   ${PROJECT_NAME}
@@ -41,9 +43,11 @@ ament_target_dependencies(
   INTERFACE geometry_msgs
             nav_msgs
             sensor_msgs
+            std_msgs
             tf2
             tf2_eigen
-            tf2_geometry_msgs)
+            tf2_geometry_msgs
+            visualization_msgs)
 target_compile_definitions(beluga_ros INTERFACE BELUGA_ROS_VERSION=2)
 target_compile_features(beluga_ros INTERFACE cxx_std_17)
 
@@ -52,9 +56,11 @@ ament_export_dependencies(
   geometry_msgs
   nav_msgs
   sensor_msgs
+  std_msgs
   tf2
   tf2_eigen
-  tf2_geometry_msgs)
+  tf2_geometry_msgs
+  visualization_msgs)
 ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_definitions(BELUGA_ROS_VERSION=2)
 ament_export_targets(beluga_ros HAS_LIBRARY_TARGET)

diff --git a/beluga_ros/cmake/catkin.cmake b/beluga_ros/cmake/catkin.cmake
@@ -17,17 +17,21 @@ find_package(
   catkin REQUIRED
   COMPONENTS nav_msgs
              sensor_msgs
+             std_msgs
              tf2
              tf2_eigen
-             tf2_geometry_msgs)
+             tf2_geometry_msgs
+             visualization_msgs)
 
 catkin_package(
   CATKIN_DEPENDS
     nav_msgs
     sensor_msgs
+    std_msgs
     tf2
     tf2_eigen
     tf2_geometry_msgs
+    visualization_msgs
   DEPENDS beluga
   INCLUDE_DIRS include
   CFG_EXTRAS ${PROJECT_NAME}-extras.cmake.in)

diff --git a/beluga_ros/docs/_doxygen/beluga_ros-main.md b/beluga_ros/docs/_doxygen/beluga_ros-main.md
@@ -27,4 +27,6 @@ Thin, Beluga-compatible wrappers for typical ROS data structures (usually messag
 
 | | |
 |-|-|
+| [particle_cloud.hpp](@ref particle_cloud.hpp) | APIs for particle cloud I/O over ROS interfaces. |
+| [tf2_eigen.hpp](@ref tf2_eigen.hpp) | tf2 message conversion API overloads for Eigen types. |
 | [tf2_sophus.hpp](@ref tf2_sophus.hpp) | tf2 message conversion API overloads for Sophus types. |
diff --git a/beluga_ros/include/beluga_ros/beluga_ros.hpp b/beluga_ros/include/beluga_ros/beluga_ros.hpp
@@ -28,6 +28,8 @@
 #include <beluga_ros/amcl.hpp>
 #include <beluga_ros/laser_scan.hpp>
 #include <beluga_ros/occupancy_grid.hpp>
+#include <beluga_ros/particle_cloud.hpp>
+#include <beluga_ros/tf2_eigen.hpp>
 #include <beluga_ros/tf2_sophus.hpp>
 
 #endif
diff --git a/beluga_ros/include/beluga_ros/messages.hpp b/beluga_ros/include/beluga_ros/messages.hpp
@@ -16,19 +16,27 @@
 #define BELUGA_ROS_MESSAGES_HPP
 
 #if BELUGA_ROS_VERSION == 2
+#include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/transform.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <rclcpp/time.hpp>
 #elif BELUGA_ROS_VERSION == 1
+#include <geometry_msgs/Point.h>
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/PoseArray.h>
 #include <geometry_msgs/Transform.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <sensor_msgs/LaserScan.h>
+#include <std_msgs/ColorRGBA.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
 
 #include <ros/time.h>
 #else
@@ -42,20 +50,28 @@ namespace msg {
 
 #if BELUGA_ROS_VERSION == 2
 
+using ColorRGBA = std_msgs::msg::ColorRGBA;
 using LaserScan = sensor_msgs::msg::LaserScan;
 using LaserScanConstSharedPtr = LaserScan::ConstSharedPtr;
+using Marker = visualization_msgs::msg::Marker;
+using MarkerArray = visualization_msgs::msg::MarkerArray;
 using OccupancyGrid = nav_msgs::msg::OccupancyGrid;
 using OccupancyGridConstSharedPtr = OccupancyGrid::ConstSharedPtr;
+using Point = geometry_msgs::msg::Point;
 using Pose = geometry_msgs::msg::Pose;
 using PoseArray = geometry_msgs::msg::PoseArray;
 using Transform = geometry_msgs::msg::Transform;
 
 #elif BELUGA_ROS_VERSION == 1
 
+using ColorRGBA = std_msgs::ColorRGBA;
 using LaserScan = sensor_msgs::LaserScan;
 using LaserScanConstSharedPtr = LaserScan::ConstPtr;
+using Marker = visualization_msgs::Marker;
+using MarkerArray = visualization_msgs::MarkerArray;
 using OccupancyGrid = nav_msgs::OccupancyGrid;
 using OccupancyGridConstSharedPtr = OccupancyGrid::ConstPtr;
+using Point = geometry_msgs::Point;
 using Pose = geometry_msgs::Pose;
 using PoseArray = geometry_msgs::PoseArray;
 using Transform = geometry_msgs::Transform;
@@ -86,7 +102,7 @@ using Time = ros::Time;
 /**
  * \param frame_id Frame ID to stamp the message with.
  * \param timestamp Time to stamp the message at.
- * \param[out] message Message to be assigned.
+ * \param[out] message Message to be stamped.
  * \tparam Message A message type with a header.
  */
 template <class Message>
@@ -96,6 +112,20 @@ Message& stamp_message(std::string_view frame_id, detail::Time timestamp, Messag
   return message;
 }
 
+/// Stamp all markers in a marker array message with a frame ID and timestamp.
+/**
+ * \param frame_id Frame ID to stamp markers with.
+ * \param timestamp Time to stamp markers at.
+ * \param[out] message Message to be stamped.
+ */
+inline beluga_ros::msg::MarkerArray&
+stamp_message(std::string_view frame_id, detail::Time timestamp, beluga_ros::msg::MarkerArray& message) {
+  for (auto& marker : message.markers) {
+    stamp_message(frame_id, timestamp, marker);
+  }
+  return message;
+}
+
 }  // namespace beluga_ros
 
 #endif  // BELUGA_ROS_MESSAGES_HPP