diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 6c3f436d2bdc70dfcbdcfb37af19be668360ea6f..a527a80a523ce46b23105b92110649b972125a8b 100644 --- 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" @@ -171,7 +170,6 @@ protected: 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_; /* diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index fe42cd49086dedf99160ac1a62c9ba420c3244e7..1f0ed2050d8c4ac68aa0cc7b299ff390b87f1738 100644 --- 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,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());