all publishers as unique ptrs

Signed-off-by: Steve Macenski <[email protected]>

Author: SteveMacenski

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

@@ -163,15 +163,15 @@ geometry_msgs::msg::PoseStamped SimpleChargingDock::getStagingPose(
   staging_pose.pose.orientation = tf2::toMsg(orientation);
 
   // Publish staging pose for debugging purposes
-  staging_pose_pub_->publish(staging_pose);
+  staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
   return staging_pose;
 }
 
 bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string /*id*/)
 {
   // If using not detection, set the dock pose to the static fixed-frame version
   if (!use_external_detection_pose_) {
-    dock_pose_pub_->publish(pose);
+    dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
     dock_pose_ = pose;
     return true;
   }
@@ -207,7 +207,7 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose,
 
   // Filter the detected pose
   detected = filter_->update(detected);
-  filtered_dock_pose_pub_->publish(detected);
+  filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
 
   // Rotate the just the orientation, then remove roll/pitch
   geometry_msgs::msg::PoseStamped just_orientation;
@@ -231,7 +231,7 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose,
   dock_pose_.pose.position.z = 0.0;
 
   // Publish & return dock pose for debugging purposes
-  dock_pose_pub_->publish(dock_pose_);
+  dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
   pose = dock_pose_;
   return true;
 }

nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp

@@ -144,15 +144,15 @@ geometry_msgs::msg::PoseStamped SimpleNonChargingDock::getStagingPose(
   staging_pose.pose.orientation = tf2::toMsg(orientation);
 
   // Publish staging pose for debugging purposes
-  staging_pose_pub_->publish(staging_pose);
+  staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
   return staging_pose;
 }
 
 bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string)
 {
   // If using not detection, set the dock pose to the static fixed-frame version
   if (!use_external_detection_pose_) {
-    dock_pose_pub_->publish(pose);
+    dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
     dock_pose_ = pose;
     return true;
   }
@@ -188,7 +188,7 @@ bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pos
 
   // Filter the detected pose
   detected = filter_->update(detected);
-  filtered_dock_pose_pub_->publish(detected);
+  filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
 
   // Rotate the just the orientation, then remove roll/pitch
   geometry_msgs::msg::PoseStamped just_orientation;
@@ -212,7 +212,7 @@ bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pos
   dock_pose_.pose.position.z = 0.0;
 
   // Publish & return dock pose for debugging purposes
-  dock_pose_pub_->publish(dock_pose_);
+  dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
   pose = dock_pose_;
   return true;
 }

nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp

@@ -28,7 +28,7 @@ namespace nav2_graceful_controller
    * @param motion_target Motion target in PoseStamped format
    * @return geometry_msgs::msg::PointStamped Motion target in PointStamped format
    */
-geometry_msgs::msg::PointStamped createMotionTargetMsg(
+std::unique_ptr<geometry_msgs::msg::PointStamped> createMotionTargetMsg(
   const geometry_msgs::msg::PoseStamped & motion_target);
 
 /**
@@ -39,7 +39,7 @@ geometry_msgs::msg::PointStamped createMotionTargetMsg(
    * @param slowdown_radius Radius of the slowdown circle
    * @return visualization_msgs::msg::Marker Slowdown marker
    */
-visualization_msgs::msg::Marker createSlowdownMarker(
+std::unique_ptr<visualization_msgs::msg::Marker> createSlowdownMarker(
   const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius);
 
 }  // namespace nav2_graceful_controller

nav2_graceful_controller/src/graceful_controller.cpp

@@ -233,12 +233,10 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     if (simulateTrajectory(target_pose, costmap_transform, local_plan, cmd_vel, reversing)) {
       // Successfully simulated to target_pose - compute velocity at this moment
       // Publish the selected target_pose
-      auto motion_target_point = nav2_graceful_controller::createMotionTargetMsg(target_pose);
-      motion_target_pub_->publish(motion_target_point);
+      motion_target_pub_->publish(nav2_graceful_controller::createMotionTargetMsg(target_pose));
       // Publish marker for slowdown radius around motion target for debugging / visualization
-      auto slowdown_marker = nav2_graceful_controller::createSlowdownMarker(
-        target_pose, params_->slowdown_radius);
-      slowdown_pub_->publish(slowdown_marker);
+      slowdown_pub_->publish(nav2_graceful_controller::createSlowdownMarker(
+        target_pose, params_->slowdown_radius));
       // Publish the local plan
       local_plan.header = transformed_plan.header;
       local_plan_pub_->publish(local_plan);

nav2_graceful_controller/src/utils.cpp

@@ -17,34 +17,34 @@
 namespace nav2_graceful_controller
 {
 
-geometry_msgs::msg::PointStamped createMotionTargetMsg(
+std::unique_ptr<geometry_msgs::msg::PointStamped> createMotionTargetMsg(
   const geometry_msgs::msg::PoseStamped & motion_target)
 {
-  geometry_msgs::msg::PointStamped motion_target_point;
-  motion_target_point.header = motion_target.header;
-  motion_target_point.point = motion_target.pose.position;
-  motion_target_point.point.z = 0.01;
+  auto motion_target_point = std::make_unique<geometry_msgs::msg::PointStamped>();
+  motion_target_point->header = motion_target.header;
+  motion_target_point->point = motion_target.pose.position;
+  motion_target_point->point.z = 0.01;
   return motion_target_point;
 }
 
-visualization_msgs::msg::Marker createSlowdownMarker(
+std::unique_ptr<visualization_msgs::msg::Marker> createSlowdownMarker(
   const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius)
 {
-  visualization_msgs::msg::Marker slowdown_marker;
-  slowdown_marker.header = motion_target.header;
-  slowdown_marker.ns = "slowdown";
-  slowdown_marker.id = 0;
-  slowdown_marker.type = visualization_msgs::msg::Marker::SPHERE;
-  slowdown_marker.action = visualization_msgs::msg::Marker::ADD;
-  slowdown_marker.pose = motion_target.pose;
-  slowdown_marker.pose.position.z = 0.01;
-  slowdown_marker.scale.x = slowdown_radius * 2.0;
-  slowdown_marker.scale.y = slowdown_radius * 2.0;
-  slowdown_marker.scale.z = 0.02;
-  slowdown_marker.color.a = 0.2;
-  slowdown_marker.color.r = 0.0;
-  slowdown_marker.color.g = 1.0;
-  slowdown_marker.color.b = 0.0;
+  auto slowdown_marker = std::make_unique<visualization_msgs::msg::Marker>();
+  slowdown_marker->header = motion_target.header;
+  slowdown_marker->ns = "slowdown";
+  slowdown_marker->id = 0;
+  slowdown_marker->type = visualization_msgs::msg::Marker::SPHERE;
+  slowdown_marker->action = visualization_msgs::msg::Marker::ADD;
+  slowdown_marker->pose = motion_target.pose;
+  slowdown_marker->pose.position.z = 0.01;
+  slowdown_marker->scale.x = slowdown_radius * 2.0;
+  slowdown_marker->scale.y = slowdown_radius * 2.0;
+  slowdown_marker->scale.z = 0.02;
+  slowdown_marker->color.a = 0.2;
+  slowdown_marker->color.r = 0.0;
+  slowdown_marker->color.g = 1.0;
+  slowdown_marker->color.b = 0.0;
   return slowdown_marker;
 }
 

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -59,13 +59,13 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController
 
   nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();}
 
-  geometry_msgs::msg::PointStamped createMotionTargetMsg(
+  std::unique_ptr<geometry_msgs::msg::PointStamped> createMotionTargetMsg(
     const geometry_msgs::msg::PoseStamped & motion_target)
   {
     return nav2_graceful_controller::createMotionTargetMsg(motion_target);
   }
 
-  visualization_msgs::msg::Marker createSlowdownMarker(
+  std::unique_ptr<visualization_msgs::msg::Marker> createSlowdownMarker(
     const geometry_msgs::msg::PoseStamped & motion_target)
   {
     return nav2_graceful_controller::createSlowdownMarker(
@@ -367,10 +367,10 @@ TEST(GracefulControllerTest, createMotionTargetMsg) {
   auto motion_target_msg = controller->createMotionTargetMsg(motion_target);
 
   // Check results
-  EXPECT_EQ(motion_target_msg.header.frame_id, "map");
-  EXPECT_EQ(motion_target_msg.point.x, 1.0);
-  EXPECT_EQ(motion_target_msg.point.y, 2.0);
-  EXPECT_EQ(motion_target_msg.point.z, 0.01);
+  EXPECT_EQ(motion_target_msg->header.frame_id, "map");
+  EXPECT_EQ(motion_target_msg->point.x, 1.0);
+  EXPECT_EQ(motion_target_msg->point.y, 2.0);
+  EXPECT_EQ(motion_target_msg->point.z, 0.01);
 }
 
 TEST(GracefulControllerTest, createSlowdownMsg) {
@@ -405,24 +405,24 @@ TEST(GracefulControllerTest, createSlowdownMsg) {
   auto slowdown_msg = controller->createSlowdownMarker(motion_target);
 
   // Check results
-  EXPECT_EQ(slowdown_msg.header.frame_id, "map");
-  EXPECT_EQ(slowdown_msg.ns, "slowdown");
-  EXPECT_EQ(slowdown_msg.id, 0);
-  EXPECT_EQ(slowdown_msg.type, visualization_msgs::msg::Marker::SPHERE);
-  EXPECT_EQ(slowdown_msg.action, visualization_msgs::msg::Marker::ADD);
-  EXPECT_EQ(slowdown_msg.pose.position.x, 1.0);
-  EXPECT_EQ(slowdown_msg.pose.position.y, 2.0);
-  EXPECT_EQ(slowdown_msg.pose.position.z, 0.01);
-  EXPECT_EQ(slowdown_msg.pose.orientation.x, 0.0);
-  EXPECT_EQ(slowdown_msg.pose.orientation.y, 0.0);
-  EXPECT_EQ(slowdown_msg.pose.orientation.z, 0.0);
-  EXPECT_EQ(slowdown_msg.pose.orientation.w, 1.0);
-  EXPECT_EQ(slowdown_msg.scale.x, 0.4);
-  EXPECT_EQ(slowdown_msg.scale.y, 0.4);
-  EXPECT_EQ(slowdown_msg.scale.z, 0.02);
-  EXPECT_EQ(slowdown_msg.color.r, 0.0);
-  EXPECT_EQ(slowdown_msg.color.g, 1.0);
-  EXPECT_EQ(slowdown_msg.color.b, 0.0);
+  EXPECT_EQ(slowdown_msg->header.frame_id, "map");
+  EXPECT_EQ(slowdown_msg->ns, "slowdown");
+  EXPECT_EQ(slowdown_msg->id, 0);
+  EXPECT_EQ(slowdown_msg->type, visualization_msgs::msg::Marker::SPHERE);
+  EXPECT_EQ(slowdown_msg->action, visualization_msgs::msg::Marker::ADD);
+  EXPECT_EQ(slowdown_msg->pose.position.x, 1.0);
+  EXPECT_EQ(slowdown_msg->pose.position.y, 2.0);
+  EXPECT_EQ(slowdown_msg->pose.position.z, 0.01);
+  EXPECT_EQ(slowdown_msg->pose.orientation.x, 0.0);
+  EXPECT_EQ(slowdown_msg->pose.orientation.y, 0.0);
+  EXPECT_EQ(slowdown_msg->pose.orientation.z, 0.0);
+  EXPECT_EQ(slowdown_msg->pose.orientation.w, 1.0);
+  EXPECT_EQ(slowdown_msg->scale.x, 0.4);
+  EXPECT_EQ(slowdown_msg->scale.y, 0.4);
+  EXPECT_EQ(slowdown_msg->scale.z, 0.02);
+  EXPECT_EQ(slowdown_msg->color.r, 0.0);
+  EXPECT_EQ(slowdown_msg->color.g, 1.0);
+  EXPECT_EQ(slowdown_msg->color.b, 0.0);
 }
 
 TEST(GracefulControllerTest, rotateToTarget) {

nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp

@@ -62,12 +62,12 @@ bool CollisionChecker::isCollisionImminent(
   }
 
   // visualization messages
-  nav_msgs::msg::Path arc_pts_msg;
-  arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID();
-  arc_pts_msg.header.stamp = robot_pose.header.stamp;
+  auto arc_pts_msg = std::make_unique<nav_msgs::msg::Path>();
+  arc_pts_msg->header.frame_id = costmap_ros_->getGlobalFrameID();
+  arc_pts_msg->header.stamp = robot_pose.header.stamp;
   geometry_msgs::msg::PoseStamped pose_msg;
-  pose_msg.header.frame_id = arc_pts_msg.header.frame_id;
-  pose_msg.header.stamp = arc_pts_msg.header.stamp;
+  pose_msg.header.frame_id = arc_pts_msg->header.frame_id;
+  pose_msg.header.stamp = arc_pts_msg->header.stamp;
 
   double projection_time = 0.0;
   if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) {
@@ -110,16 +110,16 @@ bool CollisionChecker::isCollisionImminent(
     pose_msg.pose.position.x = curr_pose.x;
     pose_msg.pose.position.y = curr_pose.y;
     pose_msg.pose.position.z = 0.01;
-    arc_pts_msg.poses.push_back(pose_msg);
+    arc_pts_msg->poses.push_back(pose_msg);
 
     // check for collision at the projected pose
     if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) {
-      carrot_arc_pub_->publish(arc_pts_msg);
+      carrot_arc_pub_->publish(std::move(arc_pts_msg));
       return true;
     }
   }
 
-  carrot_arc_pub_->publish(arc_pts_msg);
+  carrot_arc_pub_->publish(std::move(arc_pts_msg));
 
   return false;
 }

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -183,7 +183,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   // Transform path to robot base frame
   auto transformed_plan = path_handler_->transformGlobalPlan(
     pose, params_->max_robot_pose_search_dist, params_->interpolate_curvature_after_goal);
-  global_path_pub_->publish(transformed_plan);
+  auto plan_msg = std::make_unique<nav_msgs::msg::Path>(transformed_plan);
+  global_path_pub_->publish(std::move(plan_msg));
 
   // Find look ahead distance and point on path and publish
   double lookahead_dist = getLookAheadDistance(speed);
@@ -276,9 +277,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   }
 
   // Publish whether we are rotating to goal heading
-  std_msgs::msg::Bool is_rotating_to_heading_msg;
-  is_rotating_to_heading_msg.data = is_rotating_to_heading_;
-  is_rotating_to_heading_pub_->publish(is_rotating_to_heading_msg);
+  auto is_rotating_to_heading_msg = std::make_unique<std_msgs::msg::Bool>();
+  is_rotating_to_heading_msg->data = is_rotating_to_heading_;
+  is_rotating_to_heading_pub_->publish(std::move(is_rotating_to_heading_msg));
 
   // populate and return message
   geometry_msgs::msg::TwistStamped cmd_vel;

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -271,7 +271,8 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
 
     // Publish raw path for debug
     if (_raw_plan_publisher->get_subscription_count() > 0) {
-      _raw_plan_publisher->publish(plan);
+      auto msg = std::make_unique<nav_msgs::msg::Path>(plan);
+      _raw_plan_publisher->publish(std::move(msg));
     }
 
     return plan;
@@ -306,7 +307,8 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
 
   // Publish raw path for debug
   if (_raw_plan_publisher->get_subscription_count() > 0) {
-    _raw_plan_publisher->publish(plan);
+    auto msg = std::make_unique<nav_msgs::msg::Path>(plan);
+    _raw_plan_publisher->publish(std::move(msg));
   }
 
   // Find how much time we have left to do smoothing

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -424,7 +424,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
 
     // Publish raw path for debug
     if (_raw_plan_publisher->get_subscription_count() > 0) {
-      _raw_plan_publisher->publish(plan);
+      auto msg = std::make_unique<nav_msgs::msg::Path>(plan);
+      _raw_plan_publisher->publish(std::move(msg));
     }
 
     return plan;
@@ -444,17 +445,17 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
       _tolerance / static_cast<float>(costmap->getResolution()), cancel_checker, expansions.get()))
   {
     if (_debug_visualizations) {
-      geometry_msgs::msg::PoseArray msg;
+      auto msg = std::make_unique<geometry_msgs::msg::PoseArray>();
       geometry_msgs::msg::Pose msg_pose;
-      msg.header.stamp = _clock->now();
-      msg.header.frame_id = _global_frame;
+      msg->header.stamp = _clock->now();
+      msg->header.frame_id = _global_frame;
       for (auto & e : *expansions) {
         msg_pose.position.x = std::get<0>(e);
         msg_pose.position.y = std::get<1>(e);
         msg_pose.orientation = getWorldOrientation(std::get<2>(e));
-        msg.poses.push_back(msg_pose);
+        msg->poses.push_back(msg_pose);
       }
-      _expansions_publisher->publish(msg);
+      _expansions_publisher->publish(std::move(msg));
     }
 
     // Note: If the start is blocked only one iteration will occur before failure
@@ -479,22 +480,23 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
 
   // Publish raw path for debug
   if (_raw_plan_publisher->get_subscription_count() > 0) {
-    _raw_plan_publisher->publish(plan);
+    auto msg = std::make_unique<nav_msgs::msg::Path>(plan);
+    _raw_plan_publisher->publish(std::move(msg));
   }
 
   if (_debug_visualizations) {
     // Publish expansions for debug
-    geometry_msgs::msg::PoseArray msg;
+    auto msg = std::make_unique<geometry_msgs::msg::PoseArray>();
     geometry_msgs::msg::Pose msg_pose;
-    msg.header.stamp = _clock->now();
-    msg.header.frame_id = _global_frame;
+    msg->header.stamp = _clock->now();
+    msg->header.frame_id = _global_frame;
     for (auto & e : *expansions) {
       msg_pose.position.x = std::get<0>(e);
       msg_pose.position.y = std::get<1>(e);
       msg_pose.orientation = getWorldOrientation(std::get<2>(e));
-      msg.poses.push_back(msg_pose);
+      msg->poses.push_back(msg_pose);
     }
-    _expansions_publisher->publish(msg);
+    _expansions_publisher->publish(std::move(msg));
 
     // plot footprint path planned for debug
     if (_planned_footprints_publisher->get_subscription_count() > 0) {