Add weights to AMCL particle cloud

nav2_amcl/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(std_srvs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(nav2_msgs REQUIRED)
 
 nav2_package()
 
@@ -50,6 +51,7 @@ set(dependencies
   tf2_ros
   tf2
   nav2_util
+  nav2_msgs
 )
 
 ament_target_dependencies(${executable_name}

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -34,6 +34,8 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_amcl/motion_model/motion_model.hpp"
 #include "nav2_amcl/sensors/laser/laser.hpp"
+#include "nav2_msgs/msg/particle.hpp"
+#include "nav2_msgs/msg/particle_cloud.hpp"
 #include "nav_msgs/srv/set_map.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "std_srvs/srv/empty.hpp"
@@ -117,6 +119,8 @@ class AmclNode : public nav2_util::LifecycleNode
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     pose_pub_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr particlecloud_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
+    particle_cloud_pub_;
   void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
   void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan);
 

nav2_amcl/package.xml

@@ -32,6 +32,7 @@
   <depend>tf2_ros</depend>
   <depend>tf2</depend>
   <depend>nav2_util</depend>
+  <depend>nav2_msgs</depend>
   <depend>launch_ros</depend>
   <depend>launch_testing</depend>
 

nav2_amcl/src/amcl_node.cpp

@@ -266,6 +266,12 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
   // Lifecycle publishers must be explicitly activated
   pose_pub_->on_activate();
   particlecloud_pub_->on_activate();
+  particle_cloud_pub_->on_activate();
+
+  RCLCPP_WARN(
+    get_logger(),
+    "Publishing the particle cloud as geometry_msgs/PoseArray msg is deprecated, "
+    "will be published as nav2_msgs/ParticleCloud in the future");
 
   first_pose_sent_ = false;
 
@@ -301,6 +307,7 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   // Lifecycle publishers must be explicitly deactivated
   pose_pub_->on_deactivate();
   particlecloud_pub_->on_deactivate();
+  particle_cloud_pub_->on_deactivate();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -333,6 +340,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   // PubSub
   pose_pub_.reset();
   particlecloud_pub_.reset();
+  particle_cloud_pub_.reset();
 
   // Odometry
   motion_model_.reset();
@@ -852,17 +860,27 @@ AmclNode::publishParticleCloud(const pf_sample_set_t * set)
 {
   // If initial pose is not known, AMCL does not know the current pose
   if (!initial_pose_is_known_) {return;}
+
+  nav2_msgs::msg::ParticleCloud cloud_with_weights_msg;
+  cloud_with_weights_msg.header.stamp = this->now();
+  cloud_with_weights_msg.header.frame_id = global_frame_id_;
+  cloud_with_weights_msg.particles.resize(set->sample_count);
+
   geometry_msgs::msg::PoseArray cloud_msg;
   cloud_msg.header.stamp = this->now();
   cloud_msg.header.frame_id = global_frame_id_;
   cloud_msg.poses.resize(set->sample_count);
+
   for (int i = 0; i < set->sample_count; i++) {
     cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
     cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
     cloud_msg.poses[i].position.z = 0;
     cloud_msg.poses[i].orientation = orientationAroundZAxis(set->samples[i].pose.v[2]);
+    cloud_with_weights_msg.particles[i].pose = cloud_msg.poses[i];
+    cloud_with_weights_msg.particles[i].weight = set->samples[i].weight;
   }
   particlecloud_pub_->publish(cloud_msg);
+  particle_cloud_pub_->publish(cloud_with_weights_msg);
 }
 
 bool
@@ -1242,6 +1260,10 @@ AmclNode::initPubSub()
     "particlecloud",
     rclcpp::SensorDataQoS());
 
+  particle_cloud_pub_ = create_publisher<nav2_msgs::msg::ParticleCloud>(
+    "particle_cloud",
+    rclcpp::SensorDataQoS());
+
   pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "amcl_pose",
     rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());

nav2_msgs/CMakeLists.txt

@@ -18,6 +18,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/VoxelGrid.msg"
   "msg/BehaviorTreeStatusChange.msg"
   "msg/BehaviorTreeLog.msg"
+  "msg/Particle.msg"
+  "msg/ParticleCloud.msg"
   "srv/GetCostmap.srv"
   "srv/ClearCostmapExceptRegion.srv"
   "srv/ClearCostmapAroundRobot.srv"

nav2_msgs/msg/Particle.msg

@@ -0,0 +1,5 @@
+# This represents an individual particle with weight produced by a particle filter
+
+geometry_msgs/Pose pose
+
+float64 weight

nav2_msgs/msg/ParticleCloud.msg

@@ -0,0 +1,6 @@
+# This represents a particle cloud containing particle poses and weights
+
+std_msgs/Header header
+
+# Array of particles in the cloud
+Particle[] particles