Last additions

Signed-off-by: silanus23 <[email protected]>

Author: silanus23

nav2_behavior_tree/CMakeLists.txt

@@ -156,6 +156,9 @@ list(APPEND plugin_libs nav2_would_a_smoother_recovery_help_condition_bt_node)
 add_library(nav2_would_a_route_recovery_help_condition_bt_node SHARED plugins/condition/would_a_route_recovery_help_condition.cpp)
 list(APPEND plugin_libs nav2_would_a_route_recovery_help_condition_bt_node)
 
+add_library(nav2_is_within_path_tracking_bounds_condition_bt_node SHARED plugins/condition/is_within_path_tracking_bounds_condition.cpp)
+list(APPEND plugin_libs nav2_is_within_path_tracking_bounds_condition_bt_node)
+
 add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
 list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
 

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -367,6 +367,10 @@
       <input_port name="battery_topic">Topic for battery info</input_port>
     </Condition>
 
+    <Decorator ID="IsWithinPathTrackingBounds">
+      <input_port name="max_error">Maximum distance robot can go out in meters</input_port>
+    </Decorator>
+
     <Condition ID="DistanceTraveled">
       <input_port name="distance">Distance to check if passed</input_port>
       <input_port name="global_frame">reference frame to check in</input_port>

nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp

@@ -67,7 +67,7 @@ BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick()
     RCLCPP_ERROR(node_->get_logger(), "max_error parameter not provided");
     max_error_ = 1.0;  // Default fallback
   }
-  
+
   if (max_error_ < 0.0) {
     RCLCPP_WARN(node_->get_logger(), "max_error should be positive, using absolute value");
     max_error_ = std::abs(max_error_);

nav2_behavior_tree/test/plugins/condition/CMakeLists.txt

@@ -30,6 +30,9 @@ plugin_add_test(test_condition_is_pose_occupied test_is_pose_occupied.cpp nav2_i
 
 plugin_add_test(test_are_error_codes_present test_are_error_codes_present.cpp nav2_would_a_controller_recovery_help_condition_bt_node)
 
+plugin_add_test(test_condition_is_within_path_tracking_bounds test_is_within_path_tracking_bounds.cpp
+  nav2_is_within_path_tracking_bounds_condition_bt_node)
+
 plugin_add_test(test_would_a_controller_recovery_help
   test_would_a_controller_recovery_help.cpp
   nav2_would_a_controller_recovery_help_condition_bt_node)