Change publishers to publish unique ptrs

nav2_amcl/src/amcl_node.cpp

@@ -860,27 +860,25 @@ 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);
-
+  auto cloud_with_weights_msg = std::make_unique<nav2_msgs::msg::ParticleCloud>();
+  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);
+
+  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;
+    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);
+  particlecloud_pub_->publish(std::move(cloud_msg));
+  particle_cloud_pub_->publish(std::move(cloud_with_weights_msg));
 }
 
 bool
@@ -941,35 +939,35 @@ AmclNode::publishAmclPose(
     return;
   }
 
-  geometry_msgs::msg::PoseWithCovarianceStamped p;
+  auto p = std::make_unique<geometry_msgs::msg::PoseWithCovarianceStamped>();
   // Fill in the header
-  p.header.frame_id = global_frame_id_;
-  p.header.stamp = laser_scan->header.stamp;
+  p->header.frame_id = global_frame_id_;
+  p->header.stamp = laser_scan->header.stamp;
   // Copy in the pose
-  p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
-  p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
-  p.pose.pose.orientation = orientationAroundZAxis(hyps[max_weight_hyp].pf_pose_mean.v[2]);
+  p->pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
+  p->pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
+  p->pose.pose.orientation = orientationAroundZAxis(hyps[max_weight_hyp].pf_pose_mean.v[2]);
   // Copy in the covariance, converting from 3-D to 6-D
   pf_sample_set_t * set = pf_->sets + pf_->current_set;
   for (int i = 0; i < 2; i++) {
     for (int j = 0; j < 2; j++) {
       // Report the overall filter covariance, rather than the
       // covariance for the highest-weight cluster
-      // p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
-      p.pose.covariance[6 * i + j] = set->cov.m[i][j];
+      // p->covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
+      p->pose.covariance[6 * i + j] = set->cov.m[i][j];
     }
   }
-  p.pose.covariance[6 * 5 + 5] = set->cov.m[2][2];
+  p->pose.covariance[6 * 5 + 5] = set->cov.m[2][2];
   float temp = 0.0;
-  for (auto covariance_value : p.pose.covariance) {
+  for (auto covariance_value : p->pose.covariance) {
     temp += covariance_value;
   }
-  temp += p.pose.pose.position.x + p.pose.pose.position.y;
+  temp += p->pose.pose.position.x + p->pose.pose.position.y;
   if (!std::isnan(temp)) {
     RCLCPP_DEBUG(get_logger(), "Publishing pose");
-    pose_pub_->publish(p);
+    last_published_pose_ = *p;
     first_pose_sent_ = true;
-    last_published_pose_ = p;
+    pose_pub_->publish(std::move(p));
   } else {
     RCLCPP_WARN(
       get_logger(), "AMCL covariance or pose is NaN, likely due to an invalid "

nav2_bt_navigator/src/ros_topic_logger.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <memory>
 #include <utility>
 #include "nav2_bt_navigator/ros_topic_logger.hpp"
 #include "tf2_ros/buffer_interface.h"
@@ -56,10 +57,10 @@ void RosTopicLogger::callback(
 void RosTopicLogger::flush()
 {
   if (event_log_.size() > 0) {
-    nav2_msgs::msg::BehaviorTreeLog log_msg;
-    log_msg.timestamp = ros_node_->now();
-    log_msg.event_log = event_log_;
-    log_pub_->publish(log_msg);
+    auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>();
+    log_msg->timestamp = ros_node_->now();
+    log_msg->event_log = event_log_;
+    log_pub_->publish(std::move(log_msg));
     event_log_.clear();
   }
 }

nav2_controller/src/nav2_controller.cpp

@@ -16,6 +16,7 @@
 #include <vector>
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "nav2_core/exceptions.hpp"
 #include "nav_2d_utils/conversions.hpp"
@@ -346,8 +347,8 @@ void ControllerServer::updateGlobalPath()
 
 void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)
 {
-  auto cmd_vel = velocity.twist;
-  vel_publisher_->publish(cmd_vel);
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(velocity.twist);
+  vel_publisher_->publish(std::move(cmd_vel));
 }
 
 void ControllerServer::publishZeroVelocity()

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -151,8 +151,10 @@ class Costmap2DPublisher
   // Service for getting the costmaps
   rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
 
-  nav_msgs::msg::OccupancyGrid grid_;
-  nav2_msgs::msg::Costmap costmap_raw_;
+  float grid_resolution;
+  unsigned int grid_width, grid_height;
+  std::unique_ptr<nav_msgs::msg::OccupancyGrid> grid_;
+  std::unique_ptr<nav2_msgs::msg::Costmap> costmap_raw_;
   // Translate from 0-255 values in costmap to -1 to 100 values in message.
   static char * cost_translation_table_;
 };

nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

@@ -99,7 +99,6 @@ class VoxelLayer : public ObstacleLayer
   double z_resolution_, origin_z_;
   int unknown_threshold_, mark_threshold_, size_z_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr clearing_endpoints_pub_;
-  sensor_msgs::msg::PointCloud clearing_endpoints_;
 
   inline bool worldToMap3DFloat(
     double wx, double wy, double wz, double & mx, double & my,

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -41,6 +41,8 @@
 #include <algorithm>
 #include <cassert>
 #include <vector>
+#include <memory>
+#include <utility>
 
 #include "pluginlib/class_list_macros.hpp"
 #include "sensor_msgs/point_cloud2_iterator.hpp"
@@ -207,24 +209,25 @@ void VoxelLayer::updateBounds(
   }
 
   if (publish_voxel_) {
-    nav2_msgs::msg::VoxelGrid grid_msg;
+    auto grid_msg = std::make_unique<nav2_msgs::msg::VoxelGrid>();
     unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
-    grid_msg.size_x = voxel_grid_.sizeX();
-    grid_msg.size_y = voxel_grid_.sizeY();
-    grid_msg.size_z = voxel_grid_.sizeZ();
-    grid_msg.data.resize(size);
-    memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
-
-    grid_msg.origin.x = origin_x_;
-    grid_msg.origin.y = origin_y_;
-    grid_msg.origin.z = origin_z_;
-
-    grid_msg.resolutions.x = resolution_;
-    grid_msg.resolutions.y = resolution_;
-    grid_msg.resolutions.z = z_resolution_;
-    grid_msg.header.frame_id = global_frame_;
-    grid_msg.header.stamp = node_->now();
-    voxel_pub_->publish(grid_msg);
+    grid_msg->size_x = voxel_grid_.sizeX();
+    grid_msg->size_y = voxel_grid_.sizeY();
+    grid_msg->size_z = voxel_grid_.sizeZ();
+    grid_msg->data.resize(size);
+    memcpy(&grid_msg->data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
+
+    grid_msg->origin.x = origin_x_;
+    grid_msg->origin.y = origin_y_;
+    grid_msg->origin.z = origin_z_;
+
+    grid_msg->resolutions.x = resolution_;
+    grid_msg->resolutions.y = resolution_;
+    grid_msg->resolutions.z = z_resolution_;
+    grid_msg->header.frame_id = global_frame_;
+    grid_msg->header.stamp = node_->now();
+
+    voxel_pub_->publish(std::move(grid_msg));
   }
 
   updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
@@ -289,6 +292,8 @@ void VoxelLayer::raytraceFreespace(
   double * max_x,
   double * max_y)
 {
+  auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud>();
+
   size_t clearing_observation_cloud_size = clearing_observation.cloud_->height *
     clearing_observation.cloud_->width;
   if (clearing_observation_cloud_size == 0) {
@@ -310,8 +315,8 @@ void VoxelLayer::raytraceFreespace(
 
   bool publish_clearing_points = (node_->count_subscribers("clearing_endpoints") > 0);
   if (publish_clearing_points) {
-    clearing_endpoints_.points.clear();
-    clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
+    clearing_endpoints_->points.clear();
+    clearing_endpoints_->points.reserve(clearing_observation_cloud_size);
   }
 
   // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
@@ -391,16 +396,16 @@ void VoxelLayer::raytraceFreespace(
         point.x = wpx;
         point.y = wpy;
         point.z = wpz;
-        clearing_endpoints_.points.push_back(point);
+        clearing_endpoints_->points.push_back(point);
       }
     }
   }
 
   if (publish_clearing_points) {
-    clearing_endpoints_.header.frame_id = global_frame_;
-    clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
+    clearing_endpoints_->header.frame_id = global_frame_;
+    clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
 
-    clearing_endpoints_pub_->publish(clearing_endpoints_);
+    clearing_endpoints_pub_->publish(std::move(clearing_endpoints_));
   }
 }
 

nav2_costmap_2d/src/costmap_2d_cloud.cpp

@@ -27,6 +27,8 @@
 
 #include <string>
 #include <vector>
+#include <memory>
+#include <utility>
 
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/point_cloud.hpp"
@@ -132,17 +134,17 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
   }
 
   {
-    sensor_msgs::msg::PointCloud cloud;
-    cloud.points.resize(num_marked);
-    cloud.channels.resize(1);
-    cloud.channels[0].values.resize(num_marked);
-    cloud.channels[0].name = "rgb";
-    cloud.header.frame_id = frame_id;
-    cloud.header.stamp = stamp;
-
-    sensor_msgs::msg::ChannelFloat32 & chan = cloud.channels[0];
+    auto cloud = std::make_unique<sensor_msgs::msg::PointCloud>();
+    cloud->points.resize(num_marked);
+    cloud->channels.resize(1);
+    cloud->channels[0].values.resize(num_marked);
+    cloud->channels[0].name = "rgb";
+    cloud->header.frame_id = frame_id;
+    cloud->header.stamp = stamp;
+
+    sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0];
     for (uint32_t i = 0; i < num_marked; ++i) {
-      geometry_msgs::msg::Point32 & p = cloud.points[i];
+      geometry_msgs::msg::Point32 & p = cloud->points[i];
       float & cval = chan.values[i];
       Cell & c = g_marked[i];
 
@@ -159,21 +161,21 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
       memcpy(&cval, &col, sizeof col);
     }
 
-    pub_marked->publish(cloud);
+    pub_marked->publish(std::move(cloud));
   }
 
   {
-    sensor_msgs::msg::PointCloud cloud;
-    cloud.points.resize(num_unknown);
-    cloud.channels.resize(1);
-    cloud.channels[0].values.resize(num_unknown);
-    cloud.channels[0].name = "rgb";
-    cloud.header.frame_id = frame_id;
-    cloud.header.stamp = stamp;
-
-    sensor_msgs::msg::ChannelFloat32 & chan = cloud.channels[0];
+    auto cloud = std::make_unique<sensor_msgs::msg::PointCloud>();
+    cloud->points.resize(num_unknown);
+    cloud->channels.resize(1);
+    cloud->channels[0].values.resize(num_unknown);
+    cloud->channels[0].name = "rgb";
+    cloud->header.frame_id = frame_id;
+    cloud->header.stamp = stamp;
+
+    sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0];
     for (uint32_t i = 0; i < num_unknown; ++i) {
-      geometry_msgs::msg::Point32 & p = cloud.points[i];
+      geometry_msgs::msg::Point32 & p = cloud->points[i];
       float & cval = chan.values[i];
       Cell & c = g_unknown[i];
 
@@ -190,7 +192,7 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
       memcpy(&cval, &col, sizeof col);
     }
 
-    pub_unknown->publish(cloud);
+    pub_unknown->publish(std::move(cloud));
   }
 
   timer.end();

nav2_costmap_2d/src/costmap_2d_markers.cpp

@@ -38,6 +38,8 @@
  *********************************************************************/
 #include <string>
 #include <vector>
+#include <memory>
+#include <utility>
 
 #include "rclcpp/rclcpp.hpp"
 #include "visualization_msgs/msg/marker.hpp"
@@ -111,31 +113,31 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
     }
   }
 
-  visualization_msgs::msg::Marker m;
-  m.header.frame_id = frame_id;
-  m.header.stamp = stamp;
-  m.ns = g_node->get_namespace();
-  m.id = 0;
-  m.type = visualization_msgs::msg::Marker::CUBE_LIST;
-  m.action = visualization_msgs::msg::Marker::ADD;
-  m.pose.orientation.w = 1.0;
-  m.scale.x = x_res;
-  m.scale.y = y_res;
-  m.scale.z = z_res;
-  m.color.r = g_colors_r[nav2_voxel_grid::MARKED];
-  m.color.g = g_colors_g[nav2_voxel_grid::MARKED];
-  m.color.b = g_colors_b[nav2_voxel_grid::MARKED];
-  m.color.a = g_colors_a[nav2_voxel_grid::MARKED];
-  m.points.resize(num_markers);
+  auto m = std::make_unique<visualization_msgs::msg::Marker>();
+  m->header.frame_id = frame_id;
+  m->header.stamp = stamp;
+  m->ns = g_node->get_namespace();
+  m->id = 0;
+  m->type = visualization_msgs::msg::Marker::CUBE_LIST;
+  m->action = visualization_msgs::msg::Marker::ADD;
+  m->pose.orientation.w = 1.0;
+  m->scale.x = x_res;
+  m->scale.y = y_res;
+  m->scale.z = z_res;
+  m->color.r = g_colors_r[nav2_voxel_grid::MARKED];
+  m->color.g = g_colors_g[nav2_voxel_grid::MARKED];
+  m->color.b = g_colors_b[nav2_voxel_grid::MARKED];
+  m->color.a = g_colors_a[nav2_voxel_grid::MARKED];
+  m->points.resize(num_markers);
   for (uint32_t i = 0; i < num_markers; ++i) {
     Cell & c = g_cells[i];
-    geometry_msgs::msg::Point & p = m.points[i];
+    geometry_msgs::msg::Point & p = m->points[i];
     p.x = c.x;
     p.y = c.y;
     p.z = c.z;
   }
 
-  pub->publish(m);
+  pub->publish(std::move(m));
 
   timer.end();
   RCLCPP_INFO(