removed geometry_msgs/PoseArray in AMCL, updated rviz configs

abhishek47kashyap · abhishek47kashyap · commit 0ca93b5413b8 · 2021-04-01T00:43:46.000+09:00
diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/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"
@@ -117,7 +116,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_;
   void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
diff --git a/nav2_amcl/src/amcl_node.cpp b/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,6 @@ 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].weight = set->samples[i].weight;
-  }
-  particlecloud_pub_->publish(std::move(cloud_msg));
   particle_cloud_pub_->publish(std::move(cloud_with_weights_msg));
 }
 
@@ -1264,10 +1243,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());
diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz
@@ -214,7 +214,7 @@ Visualization Manager:
       Use Timestamp: false
       Value: true
     - Alpha: 1
-      Class: nav2_rviz_plugins/ParticleCloud
+      Class: nav2_rviz_plugins/ParticleFilter
       Color: 0; 180; 0
       Enabled: true
       Max Arrow Length: 0.3
diff --git a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz
@@ -164,7 +164,7 @@ Visualization Manager:
       Use Timestamp: false
       Value: true
     - Alpha: 1
-      Class: nav2_rviz_plugins/ParticleCloud
+      Class: nav2_rviz_plugins/ParticleFilter
       Color: 0; 180; 0
       Enabled: true
       Max Arrow Length: 0.3
