Deprecates use of geometry_msgs/PoseArray for particle cloud in AMCL (r…

…os-navigation#2281)

* removed geometry_msgs/PoseArray in AMCL, updated rviz configs

* ParticleFilter -> ParticuleCloud

* put back adding of weights and poses for particles

was incorrectly deleted in 0ca93b5

* complies with linter's max line length

ros-navigation#2281 (comment)

* reformatted file using ament_uncrustify

commandline tool used: ament_uncrustify --reformat src/navigation2/nav2_amcl/src/amcl_node.cpp

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -28,7 +28,6 @@
 #include <utility>
 #include <vector>
 
-#include "geometry_msgs/msg/pose_array.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "message_filters/subscriber.h"
 #include "nav2_util/lifecycle_node.hpp"
@@ -171,7 +170,6 @@ class AmclNode : public nav2_util::LifecycleNode
     initial_pose_sub_;
   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_;
   /*

nav2_amcl/src/amcl_node.cpp

@@ -274,14 +274,8 @@ 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;
 
   // Keep track of whether we're in the active state. We won't
@@ -318,7 +312,6 @@ 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();
 
   // destroy bond connection
@@ -354,7 +347,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   // PubSub
   pose_pub_.reset();
-  particlecloud_pub_.reset();
   particle_cloud_pub_.reset();
 
   // Odometry
@@ -873,19 +865,15 @@ AmclNode::publishParticleCloud(const pf_sample_set_t * set)
   cloud_with_weights_msg->header.frame_id = global_frame_id_;
   cloud_with_weights_msg->particles.resize(set->sample_count);
 
-  auto cloud_msg = std::make_unique<geometry_msgs::msg::PoseArray>();
-  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].pose.position.x = set->samples[i].pose.v[0];
+    cloud_with_weights_msg->particles[i].pose.position.y = set->samples[i].pose.v[1];
+    cloud_with_weights_msg->particles[i].pose.position.z = 0;
+    cloud_with_weights_msg->particles[i].pose.orientation = orientationAroundZAxis(
+      set->samples[i].pose.v[2]);
     cloud_with_weights_msg->particles[i].weight = set->samples[i].weight;
   }
-  particlecloud_pub_->publish(std::move(cloud_msg));
+
   particle_cloud_pub_->publish(std::move(cloud_with_weights_msg));
 }
 
@@ -1264,10 +1252,6 @@ AmclNode::initPubSub()
 {
   RCLCPP_INFO(get_logger(), "initPubSub");
 
-  particlecloud_pub_ = create_publisher<geometry_msgs::msg::PoseArray>(
-    "particlecloud",
-    rclcpp::SensorDataQoS());
-
   particle_cloud_pub_ = create_publisher<nav2_msgs::msg::ParticleCloud>(
     "particle_cloud",
     rclcpp::SensorDataQoS());