Fix memory growth issue in isPoseOccupied (#5715)

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Author: mini-1235

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp

@@ -60,6 +60,11 @@ class IsPoseOccupiedCondition : public BT::ConditionNode
    */
   void initialize();
 
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing node-specific ports
@@ -72,7 +77,8 @@ class IsPoseOccupiedCondition : public BT::ConditionNode
 
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("pose", "Pose to check if occupied"),
-      BT::InputPort<std::string>("service_name", "global_costmap/get_cost_global_costmap"),
+      BT::InputPort<std::string>("service_name", "global_costmap/get_cost_global_costmap",
+        "The service name to call GetCosts"),
       BT::InputPort<double>(
         "cost_threshold", 254.0,
         "Cost threshold for considering a pose occupied"),

nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp

@@ -31,13 +31,11 @@ IsPathValidCondition::IsPathValidCondition(
     node_->create_client<nav2_msgs::srv::IsPathValid>(
     "is_path_valid",
     false /* Does not create and spin an internal executor*/);
-
-  server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
 }
 
 void IsPathValidCondition::initialize()
 {
-  getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+  getInputOrBlackboard("server_timeout", server_timeout_);
   getInput<unsigned int>("max_cost", max_cost_);
   getInput<bool>("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
 }

nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp

@@ -24,24 +24,32 @@ IsPoseOccupiedCondition::IsPoseOccupiedCondition(
   const std::string & condition_name,
   const BT::NodeConfiguration & conf)
 : BT::ConditionNode(condition_name, conf),
-  use_footprint_(true), consider_unknown_as_obstacle_(false), cost_threshold_(254),
-  service_name_("global_costmap/get_cost_global_costmap")
+  use_footprint_(true), consider_unknown_as_obstacle_(false), cost_threshold_(254)
 {
-  node_ = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
-  server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+  initialize();
 }
 
 void IsPoseOccupiedCondition::initialize()
 {
-  getInput<std::string>("service_name", service_name_);
   getInput<double>("cost_threshold", cost_threshold_);
   getInput<bool>("use_footprint", use_footprint_);
   getInput<bool>("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
-  getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
-  client_ =
-    node_->create_client<nav2_msgs::srv::GetCosts>(
-    service_name_,
-    false /* Does not create and spin an internal executor*/);
+  getInputOrBlackboard("server_timeout", server_timeout_);
+  createROSInterfaces();
+}
+
+void IsPoseOccupiedCondition::createROSInterfaces()
+{
+  std::string service_new;
+  getInput<std::string>("service_name", service_new);
+  if (service_new != service_name_ || !client_) {
+    service_name_ = service_new;
+    node_ = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
+    client_ =
+      node_->create_client<nav2_msgs::srv::GetCosts>(
+      service_name_,
+      false /* Does not create and spin an internal executor*/);
+  }
 }
 
 BT::NodeStatus IsPoseOccupiedCondition::tick()