Deleting unnecessary complicated tests, adding necessary condition nodes and their tests

Signed-off-by: silanus23 <berkantali23@outlook.com>

Author: silanus23

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -77,7 +77,9 @@ class BtActionNode : public BT::ActionNodeBase
     if (getInput("server_name", remapped_action_name)) {
       action_name_ = remapped_action_name;
     }
-    is_global_ = config().blackboard->template get<bool>("global_mode");
+    if (!config().blackboard->template get<bool>("is_global", is_global_)) {
+      is_global_ = false;
+    }
 
     createActionClient(action_name_);
 
@@ -212,7 +214,7 @@ class BtActionNode : public BT::ActionNodeBase
         new_run_id = config().blackboard->get<std::string>("run_id");
       } catch (const std::exception & e) {
         throw std::runtime_error(
-          "global_mode=true requires 'run_id' to be set on the blackboard for action: " +
+          "is_global=true requires 'run_id' to be set on the blackboard for action: " +
           action_name_ + ". Error: " + e.what());
       }
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -171,8 +171,8 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
     node->declare_or_get_parameter("wait_for_service_timeout", 1000));
 
   // Get parameter for global node reinitialization behavior
-  is_global_ = 
-  node->declare_or_get_parameter("global_mode", true);
+  is_global_ =
+    node->declare_or_get_parameter("is_global", true);
 
   always_reload_bt_ = node->declare_or_get_parameter(
     "always_reload_bt_xml", false);
@@ -190,7 +190,7 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
   blackboard_ = BT::Blackboard::create();
 
   // Put items on the blackboard
-  blackboard_->template set<bool>("global_mode", is_global_);  // NOLINT
+  blackboard_->template set<bool>("is_global", is_global_);  // NOLINT
   blackboard_->template set<nav2::LifecycleNode::SharedPtr>("node", client_node_);  // NOLINT
   blackboard_->template set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  // NOLINT
   blackboard_->template set<std::chrono::milliseconds>("cancel_timeout", default_cancel_timeout_);  // NOLINT
@@ -384,7 +384,7 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
       blackboard->template set<std::chrono::milliseconds>(
         "wait_for_service_timeout",
         wait_for_service_timeout_);
-      blackboard->template set<bool>("global_mode", is_global_);
+      blackboard->template set<bool>("is_global", is_global_);
     }
   } catch (const std::exception & e) {
     setInternalError(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp

@@ -81,6 +81,9 @@ class DistanceTraveledCondition : public BT::ConditionNode
   double distance_;
   double transform_tolerance_;
   std::string global_frame_, robot_base_frame_;
+
+  bool is_global_;
+  std::string current_run_id_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp

@@ -53,6 +53,11 @@ class GoalUpdatedCondition : public BT::ConditionNode
    */
   BT::NodeStatus tick() override;
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing node-specific ports
@@ -74,6 +79,8 @@ class GoalUpdatedCondition : public BT::ConditionNode
 private:
   geometry_msgs::msg::PoseStamped goal_;
   nav_msgs::msg::Goals goals_;
+  bool is_global_;
+  std::string current_run_id_;
 };
 
 }  // namespace nav2_behavior_tree

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

@@ -86,6 +86,9 @@ class IsBatteryChargingCondition : public BT::ConditionNode
   std::string battery_topic_;
   bool is_battery_charging_;
   std::chrono::milliseconds bt_loop_duration_;
+
+  bool is_global_;
+  std::string current_run_id_;
 };
 
 }  // namespace nav2_behavior_tree

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

@@ -94,6 +94,9 @@ class IsBatteryLowCondition : public BT::ConditionNode
   bool is_voltage_;
   bool is_battery_low_;
   std::chrono::milliseconds bt_loop_duration_;
+
+  bool is_global_;
+  std::string current_run_id_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp

@@ -69,6 +69,9 @@ class TimeExpiredCondition : public BT::ConditionNode
   nav2::LifecycleNode::SharedPtr node_;
   rclcpp::Time start_;
   double period_;
+
+  bool is_global_;
+  std::string current_run_id_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp

@@ -81,6 +81,9 @@ class TransformAvailableCondition : public BT::ConditionNode
 
   std::string child_frame_;
   std::string parent_frame_;
+
+  bool is_global_;
+  std::string current_run_id_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/package.xml

@@ -13,6 +13,7 @@
 
   <depend>action_msgs</depend>
   <depend>backward_ros</depend>
+  <depend>libboost-dev</depend>
   <depend>behaviortree_cpp</depend>
   <depend>geometry_msgs</depend>
   <depend>nav2_common</depend>

nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

@@ -29,7 +29,9 @@ DistanceTraveledCondition::DistanceTraveledCondition(
   const BT::NodeConfiguration & conf)
 : BT::ConditionNode(condition_name, conf),
   distance_(1.0),
-  transform_tolerance_(0.1)
+  transform_tolerance_(0.1),
+  is_global_(false),
+  current_run_id_("")
 {
 }
 
@@ -45,19 +47,45 @@ void DistanceTraveledCondition::initialize()
     node_, "global_frame", this);
   robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node_, "robot_base_frame", this);
+  is_global_ = node_->declare_or_get_parameter("is_global", false);
 }
 
 BT::NodeStatus DistanceTraveledCondition::tick()
 {
   if (!BT::isStatusActive(status())) {
     initialize();
-    if (!nav2_util::getCurrentPose(
-        start_pose_, *tf_, global_frame_, robot_base_frame_,
-        transform_tolerance_))
-    {
-      RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    if (!is_global_) {
+      if (!nav2_util::getCurrentPose(
+          start_pose_, *tf_, global_frame_, robot_base_frame_,
+          transform_tolerance_))
+      {
+        RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+      }
+      return BT::NodeStatus::FAILURE;
+    } else if (start_pose_.header.frame_id.empty()) {
+      if (!nav2_util::getCurrentPose(
+          start_pose_, *tf_, global_frame_, robot_base_frame_,
+          transform_tolerance_))
+      {
+        RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+      }
+    }
+  }
+
+  // Global mode: on RunID change fall through without resetting start_pose_
+  if (is_global_) {
+    std::string new_run_id;
+    try {
+      new_run_id = config().blackboard->template get<std::string>("run_id");
+    } catch (const std::exception & e) {
+      throw std::runtime_error(
+        "is_global=true requires 'run_id' to be set on the blackboard for condition: " +
+        std::string(name()));
+    }
+    if (new_run_id != current_run_id_) {
+      current_run_id_ = new_run_id;
+      // Do not reset start_pose_ to preserve distance measurement across runs
     }
-    return BT::NodeStatus::FAILURE;
   }
 
   // Determine distance travelled since we've started this iteration