Add initialize()

Author: Tacha-S

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -50,17 +50,6 @@ class BtServiceNode : public BT::ActionNodeBase
   : BT::ActionNodeBase(service_node_name, conf), service_name_(service_name), service_node_name_(
       service_node_name)
   {
-    // Get the required items from the blackboard
-    auto bt_loop_duration =
-      config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
-    getInputOrBlackboard("server_timeout", server_timeout_);
-    wait_for_service_timeout_ =
-      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
-
-    // timeout should be less than bt_loop_duration to be able to finish the current tick
-    max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
-
-    // Now that we have node_ to use, create the service client for this BT service
     createROSInterfaces();
 
     // Make a request for the service without parameter
@@ -91,22 +80,46 @@ class BtServiceNode : public BT::ActionNodeBase
   {
   }
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize()
+  {
+    // Get the required items from the blackboard
+    auto bt_loop_duration =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
+    getInputOrBlackboard("server_timeout", server_timeout_);
+    wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
+
+    // timeout should be less than bt_loop_duration to be able to finish the current tick
+    max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);
+
+    // Now that we have node_ to use, create the service client for this BT service
+    createROSInterfaces();
+  }
+
   /**
    * @brief Function to create ROS interfaces
    */
   void createROSInterfaces()
   {
-    node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
-    callback_group_ = node_->create_callback_group(
-      rclcpp::CallbackGroupType::MutuallyExclusive,
-      false);
-    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
-
-    getInput("service_name", service_name_);
-    service_client_ = node_->create_client<ServiceT>(
-      service_name_,
-      rclcpp::SystemDefaultsQoS(),
-      callback_group_);
+    std::string service_new;
+    getInput("service_name", service_new);
+    if (service_new != service_name_ || !service_client_) {
+      service_name_ = service_new;
+      node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
+      callback_group_ = node_->create_callback_group(
+        rclcpp::CallbackGroupType::MutuallyExclusive,
+        false);
+      callback_group_executor_.add_callback_group(callback_group_,
+          node_->get_node_base_interface());
+
+      service_client_ = node_->create_client<ServiceT>(
+        service_name_,
+        rclcpp::SystemDefaultsQoS(),
+        callback_group_);
+    }
   }
 
   /**
@@ -141,6 +154,10 @@ class BtServiceNode : public BT::ActionNodeBase
    */
   BT::NodeStatus tick() override
   {
+    if (!BT::isStatusActive(status())) {
+      initialize();
+    }
+
     if (!request_sent_) {
       // reset the flag to send the request or not,
       // allowing the user the option to set it in on_tick

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp

@@ -71,6 +71,10 @@ class ControllerSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
   /**
    * @brief Function to create ROS interfaces
    */

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp

@@ -71,6 +71,10 @@ class GoalCheckerSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
   /**
    * @brief Function to create ROS interfaces
    */

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp

@@ -71,6 +71,10 @@ class PlannerSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
   /**
    * @brief Function to create ROS interfaces
    */

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp

@@ -70,6 +70,10 @@ class ProgressCheckerSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
   /**
    * @brief Function to create ROS interfaces
    */

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp

@@ -71,6 +71,10 @@ class SmootherSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
   /**
    * @brief Function to create ROS interfaces
    */

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp

@@ -63,6 +63,10 @@ class IsBatteryChargingCondition : public BT::ConditionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
   /**
    * @brief Function to create ROS interfaces
    */

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

@@ -68,10 +68,6 @@ class IsPathValidCondition : public BT::ConditionNode
   }
 
 private:
-  /**
-   * @brief Function to create ROS interfaces
-   */
-  void createROSInterfaces();
   rclcpp::Node::SharedPtr node_;
   rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
   // The timeout value while waiting for a response from the

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp

@@ -84,10 +84,6 @@ class IsStuckCondition : public BT::ConditionNode
   static BT::PortsList providedPorts() {return {};}
 
 private:
-  /**
-   * @brief Function to create ROS interfaces
-   */
-  void createROSInterfaces();
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

@@ -63,6 +63,10 @@ class GoalUpdater : public BT::DecoratorNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
   /**
    * @brief Function to create ROS interfaces
    */
@@ -96,6 +100,8 @@ class GoalUpdater : public BT::DecoratorNode
   rclcpp::Node::SharedPtr node_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+  std::string goal_updater_topic_;
+  std::string goals_updater_topic_;
 };
 
 }  // namespace nav2_behavior_tree