Fix memory leak (#1900)

* Fix memory leak in nav2_recoveries

* Fix recovery server memory leak (better interface)

* Fix costmap2d memory leak

* Fix nav2_navfn_planner memory leak

* Fix planner server and navfn planner memory leak

* Make all rclcpp::Node::SharedPtr argument passing const

* Fix controller server and DWB plugins memory leak

* Minor fixes

* Fix formatting errors

* Change all plugin interfaces to use weak_ptr intead of shared_ptr to parent rclcpp::Node

* Convert all SharedPtr to WeakPtr

* Check shared_ptr after lock and before dereferencing

Author: naiveHobo

nav2_amcl/src/amcl_node.cpp

@@ -225,7 +225,6 @@ AmclNode::AmclNode()
 
 AmclNode::~AmclNode()
 {
-  RCLCPP_INFO(get_logger(), "Destroying");
 }
 
 nav2_util::CallbackReturn

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -69,6 +69,8 @@ bt_navigator:
     - nav2_rate_controller_bt_node
     - nav2_distance_controller_bt_node
     - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_transform_available_condition_bt_node

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -69,6 +69,8 @@ bt_navigator:
     - nav2_rate_controller_bt_node
     - nav2_distance_controller_bt_node
     - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_transform_available_condition_bt_node

nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp

@@ -27,7 +27,7 @@ namespace nav2_bt_navigator
 class RosTopicLogger : public BT::StatusChangeLogger
 {
 public:
-  RosTopicLogger(const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree);
+  RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree);
 
   void callback(
     BT::Duration timestamp,
@@ -38,7 +38,8 @@ class RosTopicLogger : public BT::StatusChangeLogger
   void flush() override;
 
 protected:
-  rclcpp::Node::SharedPtr ros_node_;
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Logger logger_{rclcpp::get_logger("bt_navigator")};
   rclcpp::Publisher<nav2_msgs::msg::BehaviorTreeLog>::SharedPtr log_pub_;
   std::vector<nav2_msgs::msg::BehaviorTreeStatusChange> event_log_;
 };

nav2_bt_navigator/src/bt_navigator.cpp

@@ -16,7 +16,6 @@
 
 #include <fstream>
 #include <memory>
-#include <streambuf>
 #include <string>
 #include <utility>
 #include <vector>
@@ -52,7 +51,7 @@ BtNavigator::BtNavigator()
     "nav2_distance_controller_bt_node",
     "nav2_speed_controller_bt_node",
     "nav2_truncate_path_action_bt_node",
-    "nav2_change_goal_node_bt_node",
+    "nav2_goal_updater_node_bt_node",
     "nav2_recovery_node_bt_node",
     "nav2_pipeline_sequence_bt_node",
     "nav2_round_robin_node_bt_node",
@@ -72,7 +71,6 @@ BtNavigator::BtNavigator()
 
 BtNavigator::~BtNavigator()
 {
-  RCLCPP_INFO(get_logger(), "Destroying");
 }
 
 nav2_util::CallbackReturn
@@ -246,7 +244,7 @@ BtNavigator::navigateToPose()
   auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree;
 
   // Empty id in request is default for backward compatibility
-  bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename;
+  bt_xml_filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
 
   if (!loadBehaviorTree(bt_xml_filename)) {
     RCLCPP_ERROR(

nav2_bt_navigator/src/ros_topic_logger.cpp

@@ -21,10 +21,13 @@ namespace nav2_bt_navigator
 {
 
 RosTopicLogger::RosTopicLogger(
-  const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree)
-: StatusChangeLogger(tree.rootNode()), ros_node_(ros_node)
+  const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree)
+: StatusChangeLogger(tree.rootNode())
 {
-  log_pub_ = ros_node_->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
+  auto node = ros_node.lock();
+  clock_ = node->get_clock();
+  logger_ = node->get_logger();
+  log_pub_ = node->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
     "behavior_tree_log",
     rclcpp::QoS(10));
 }
@@ -51,7 +54,7 @@ void RosTopicLogger::callback(
   event_log_.push_back(std::move(event));
 
   RCLCPP_DEBUG(
-    ros_node_->get_logger(), "[%.3f]: %25s %s -> %s",
+    logger_, "[%.3f]: %25s %s -> %s",
     std::chrono::duration<double>(timestamp).count(),
     node.name().c_str(),
     toStr(prev_status, true).c_str(),
@@ -60,9 +63,9 @@ void RosTopicLogger::callback(
 
 void RosTopicLogger::flush()
 {
-  if (event_log_.size() > 0) {
+  if (!event_log_.empty()) {
     auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>();
-    log_msg->timestamp = ros_node_->now();
+    log_msg->timestamp = clock_->now();
     log_msg->event_log = event_log_;
     log_pub_->publish(std::move(log_msg));
     event_log_.clear();

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -58,7 +58,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
   SimpleGoalChecker();
   // Standard GoalChecker Interface
   void initialize(
-    const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh,
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     const std::string & plugin_name) override;
   void reset() override;
   bool isGoalReached(

nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp

@@ -34,7 +34,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker
 {
 public:
   void initialize(
-    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     const std::string & plugin_name) override;
   bool check(geometry_msgs::msg::PoseStamped & current_pose) override;
   void reset() override;
@@ -52,7 +52,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker
    */
   void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose);
 
-  rclcpp_lifecycle::LifecycleNode::SharedPtr nh_;
+  rclcpp::Clock::SharedPtr clock_;
 
   double radius_;
   rclcpp::Duration time_allowance_{0, 0};

nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp

@@ -55,7 +55,7 @@ class StoppedGoalChecker : public SimpleGoalChecker
   StoppedGoalChecker();
   // Standard GoalChecker Interface
   void initialize(
-    const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh,
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     const std::string & plugin_name) override;
   bool isGoalReached(
     const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,

nav2_controller/plugins/simple_goal_checker.cpp

@@ -56,22 +56,24 @@ SimpleGoalChecker::SimpleGoalChecker()
 }
 
 void SimpleGoalChecker::initialize(
-  const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh,
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
   const std::string & plugin_name)
 {
+  auto node = parent.lock();
+
   nav2_util::declare_parameter_if_not_declared(
-    nh,
+    node,
     plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));
   nav2_util::declare_parameter_if_not_declared(
-    nh,
+    node,
     plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25));
   nav2_util::declare_parameter_if_not_declared(
-    nh,
+    node,
     plugin_name + ".stateful", rclcpp::ParameterValue(true));
 
-  nh->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
-  nh->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_);
-  nh->get_parameter(plugin_name + ".stateful", stateful_);
+  node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
+  node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_);
+  node->get_parameter(plugin_name + ".stateful", stateful_);
 
   xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
 }