Create subscriber on constructor (ros-navigation#4859)

* Create subscriber on constructor

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>

* Get BT input topic name

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>

* Add createROSInterfaces()

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>

* Add initialize()

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>

* Fix bug

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>

* Call initialize where input port updates occur

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>

* Call initialize() on constructor

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>

---------

Signed-off-by: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>
Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

Author: Tacha-S

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -50,28 +50,7 @@ class BtServiceNode : public BT::ActionNodeBase
   : BT::ActionNodeBase(service_node_name, conf), service_name_(service_name), service_node_name_(
       service_node_name)
   {
-    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());
-
-    // 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
-    getInput("service_name", service_name_);
-    service_client_ = node_->create_client<ServiceT>(
-      service_name_,
-      rclcpp::SystemDefaultsQoS(),
-      callback_group_);
+    initialize();
 
     // Make a request for the service without parameter
     request_ = std::make_shared<typename ServiceT::Request>();
@@ -101,6 +80,48 @@ 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()
+  {
+    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_);
+    }
+  }
+
   /**
    * @brief Any subclass of BtServiceNode that accepts parameters must provide a
    * providedPorts method and call providedBasicPorts in it.
@@ -133,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,15 @@ class ControllerSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
+
   /**
    * @brief Function to perform some user-defined operation on tick
    */

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

@@ -71,6 +71,15 @@ class GoalCheckerSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
+
   /**
    * @brief Function to perform some user-defined operation on tick
    */

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

@@ -71,6 +71,15 @@ class PlannerSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
+
   /**
    * @brief Function to perform some user-defined operation on tick
    */

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

@@ -70,6 +70,15 @@ class ProgressCheckerSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
+
   /**
    * @brief Function to perform some user-defined operation on tick
    */

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

@@ -71,6 +71,15 @@ class SmootherSelector : public BT::SyncActionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
+
   /**
    * @brief Function to perform some user-defined operation on tick
    */

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

@@ -63,6 +63,15 @@ class IsBatteryChargingCondition : public BT::ConditionNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
+
   /**
    * @brief Callback function for battery topic
    * @param msg Shared pointer to sensor_msgs::msg::BatteryState message

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp

@@ -56,6 +56,11 @@ class IsBatteryLowCondition : 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

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

@@ -63,6 +63,14 @@ class GoalUpdater : public BT::DecoratorNode
   }
 
 private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
   /**
    * @brief The main override required by a BT action
    * @return BT::NodeStatus Status of tick execution
@@ -92,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

nav2_behavior_tree/plugins/action/controller_selector_node.cpp

@@ -32,28 +32,46 @@ ControllerSelector::ControllerSelector(
   const BT::NodeConfiguration & conf)
 : BT::SyncActionNode(name, conf)
 {
-  node_ = config().blackboard->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("topic_name", topic_name_);
-
-  rclcpp::QoS qos(rclcpp::KeepLast(1));
-  qos.transient_local().reliable();
-
-  rclcpp::SubscriptionOptions sub_option;
-  sub_option.callback_group = callback_group_;
-  controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
-    topic_name_,
-    qos,
-    std::bind(&ControllerSelector::callbackControllerSelect, this, _1),
-    sub_option);
+  initialize();
+}
+
+void ControllerSelector::initialize()
+{
+  createROSInterfaces();
+}
+
+void ControllerSelector::createROSInterfaces()
+{
+  std::string topic_new;
+  getInput("topic_name", topic_new);
+  if (topic_new != topic_name_ || !controller_selector_sub_) {
+    topic_name_ = topic_new;
+    node_ = config().blackboard->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());
+
+
+    rclcpp::QoS qos(rclcpp::KeepLast(1));
+    qos.transient_local().reliable();
+
+    rclcpp::SubscriptionOptions sub_option;
+    sub_option.callback_group = callback_group_;
+    controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+      topic_name_,
+      qos,
+      std::bind(&ControllerSelector::callbackControllerSelect, this, _1),
+      sub_option);
+  }
 }
 
 BT::NodeStatus ControllerSelector::tick()
 {
+  if (!BT::isStatusActive(status())) {
+    initialize();
+  }
+
   callback_group_executor_.spin_some();
 
   // This behavior always use the last selected controller received from the topic input.