Merge remote-tracking branch 'origin/main' into use_posestampedarray

Signed-off-by: Tony Najjar <[email protected]>
ros-navigation · Dec 11, 2024 · 7c082e7 · 7c082e7
2 parents bdef481 + c0ba65d
commit 7c082e7
Show file tree

Hide file tree

Showing 24 changed files with 237 additions and 106 deletions.
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
@@ -80,6 +80,7 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
         BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
+        BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The back up behavior server error code")
       });

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp
@@ -59,6 +59,7 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
         BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"),
         BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
+        BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
         BT::OutputPort<Action::Result::_error_code_type>(
           "error_code_id", "The drive on heading behavior server error code")
       });

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp
@@ -64,6 +64,7 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
         BT::InputPort<double>("spin_dist", 1.57, "Spin distance"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning"),
         BT::InputPort<bool>("is_recovery", true, "True if recovery"),
+        BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The spin behavior error code")
       });

diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -13,6 +13,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="disable_collision_checks">Disable collision checking</input_port>
       <output_port name="error_code_id">"Back up error code"</output_port>
     </Action>
 
@@ -22,6 +23,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="disable_collision_checks">Disable collision checking</input_port>
       <output_port name="error_code_id">"Drive on heading error code"</output_port>
     </Action>
 
@@ -201,6 +203,7 @@
       <input_port name="time_allowance">Allowed time for spinning</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="disable_collision_checks">Disable collision checking</input_port>
       <output_port name="error_code_id">Spin error code</output_port>
     </Action>
 

diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp
@@ -36,13 +36,16 @@ void nav2_behavior_tree::BackUpAction::initialize()
   getInput("backup_speed", speed);
   double time_allowance;
   getInput("time_allowance", time_allowance);
+  bool disable_collision_checks;
+  getInput("disable_collision_checks", disable_collision_checks);
 
   // Populate the input message
   goal_.target.x = dist;
   goal_.target.y = 0.0;
   goal_.target.z = 0.0;
   goal_.speed = speed;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+  goal_.disable_collision_checks = disable_collision_checks;
 }
 
 void BackUpAction::on_tick()

diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
@@ -37,13 +37,16 @@ void DriveOnHeadingAction::initialize()
   getInput("speed", speed);
   double time_allowance;
   getInput("time_allowance", time_allowance);
+  bool disable_collision_checks;
+  getInput("disable_collision_checks", disable_collision_checks);
 
   // Populate the input message
   goal_.target.x = dist;
   goal_.target.y = 0.0;
   goal_.target.z = 0.0;
   goal_.speed = speed;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+  goal_.disable_collision_checks = disable_collision_checks;
   initalized_ = true;
 }
 

diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
@@ -55,6 +55,14 @@ void RemoveInCollisionGoals::on_tick()
 BT::NodeStatus RemoveInCollisionGoals::on_completion(
   std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
 {
+  if (!response->success) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "GetCosts service call failed");
+    setOutput("output_goals", input_goals_);
+    return BT::NodeStatus::FAILURE;
+  }
+
   nav2_msgs::msg::PoseStampedArray valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
     if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||

diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp
@@ -37,83 +37,83 @@ BT::NodeStatus RecoveryNode::tick()
     throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
   }
 
-  if (retry_count_ > number_of_retries_) {
-    halt();
-    return BT::NodeStatus::FAILURE;
-  }
   setStatus(BT::NodeStatus::RUNNING);
 
-  TreeNode * child_node = children_nodes_[current_child_idx_];
-  const BT::NodeStatus child_status = child_node->executeTick();
-
-  if (current_child_idx_ == 0) {
-    switch (child_status) {
-      case BT::NodeStatus::SKIPPED:
-        // If first child is skipped, the entire branch is considered skipped
-        halt();
-        return BT::NodeStatus::SKIPPED;
-
-      case BT::NodeStatus::SUCCESS:
-        // reset node and return success when first child returns success
-        halt();
-        return BT::NodeStatus::SUCCESS;
-
-      case BT::NodeStatus::RUNNING:
-        return BT::NodeStatus::RUNNING;
-
-      case BT::NodeStatus::FAILURE:
-        {
-          if (retry_count_ < number_of_retries_) {
-            // halt first child and tick second child in next iteration
-            ControlNode::haltChild(0);
-            current_child_idx_ = 1;
-            return BT::NodeStatus::RUNNING;
-          } else {
-            // reset node and return failure when max retries has been exceeded
-            halt();
+  while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) {
+    TreeNode * child_node = children_nodes_[current_child_idx_];
+    const BT::NodeStatus child_status = child_node->executeTick();
+
+    if (current_child_idx_ == 0) {
+      switch (child_status) {
+        case BT::NodeStatus::SKIPPED:
+          // If first child is skipped, the entire branch is considered skipped
+          halt();
+          return BT::NodeStatus::SKIPPED;
+
+        case BT::NodeStatus::SUCCESS:
+          // reset node and return success when first child returns success
+          halt();
+          return BT::NodeStatus::SUCCESS;
+
+        case BT::NodeStatus::RUNNING:
+          return BT::NodeStatus::RUNNING;
+
+        case BT::NodeStatus::FAILURE:
+          {
+            if (retry_count_ < number_of_retries_) {
+              // halt first child and tick second child in next iteration
+              ControlNode::haltChild(0);
+              current_child_idx_++;
+              break;
+            } else {
+              // reset node and return failure when max retries has been exceeded
+              halt();
+              return BT::NodeStatus::FAILURE;
+            }
+          }
+
+        default:
+          throw BT::LogicError("A child node must never return IDLE");
+      }  // end switch
+
+    } else if (current_child_idx_ == 1) {
+      switch (child_status) {
+        case BT::NodeStatus::SKIPPED:
+          {
+            // if we skip the recovery (maybe a precondition fails), then we
+            // should assume that no recovery is possible. For this reason,
+            // we should return FAILURE and reset the index.
+            // This does not count as a retry.
+            current_child_idx_ = 0;
+            ControlNode::haltChild(1);
             return BT::NodeStatus::FAILURE;
           }
-        }
-
-      default:
-        throw BT::LogicError("A child node must never return IDLE");
-    }  // end switch
-
-  } else if (current_child_idx_ == 1) {
-    switch (child_status) {
-      case BT::NodeStatus::SKIPPED:
-        {
-          // if we skip the recovery (maybe a precondition fails), then we
-          // should assume that no recovery is possible. For this reason,
-          // we should return FAILURE and reset the index.
-          // This does not count as a retry.
-          current_child_idx_ = 0;
-          ControlNode::haltChild(1);
+        case BT::NodeStatus::RUNNING:
+          return child_status;
+
+        case BT::NodeStatus::SUCCESS:
+          {
+            // halt second child, increment recovery count, and tick first child in next iteration
+            ControlNode::haltChild(1);
+            retry_count_++;
+            current_child_idx_ = 0;
+          }
+          break;
+
+        case BT::NodeStatus::FAILURE:
+          // reset node and return failure if second child fails
+          halt();
           return BT::NodeStatus::FAILURE;
-        }
-      case BT::NodeStatus::RUNNING:
-        return child_status;
-
-      case BT::NodeStatus::SUCCESS:
-        {
-          // halt second child, increment recovery count, and tick first child in next iteration
-          ControlNode::haltChild(1);
-          retry_count_++;
-          current_child_idx_ = 0;
-          return BT::NodeStatus::RUNNING;
-        }
 
-      case BT::NodeStatus::FAILURE:
-        // reset node and return failure if second child fails
-        halt();
-        return BT::NodeStatus::FAILURE;
+        default:
+          throw BT::LogicError("A child node must never return IDLE");
+      }  // end switch
+    }
+  }  // end while loop
 
-      default:
-        throw BT::LogicError("A child node must never return IDLE");
-    }  // end switch
-  }
+  // reset node and return failure
   halt();
-  throw BT::LogicError("A recovery node has exactly 2 children and should never reach here");
+  return BT::NodeStatus::FAILURE;
 }
 
 void RecoveryNode::halt()

diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp
@@ -131,18 +131,20 @@ TEST_F(BackUpActionTestFixture, test_ports)
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 0.15);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.025);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), false);
 
   xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <BackUp backup_dist="2" backup_speed="0.26" />
+            <BackUp backup_dist="2" backup_speed="0.26" disable_collision_checks="true" />
         </BehaviorTree>
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 2.0);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.26);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), true);
 }
 
 TEST_F(BackUpActionTestFixture, test_tick)

diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp
@@ -128,18 +128,20 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports)
   EXPECT_EQ(tree_->rootNode()->getInput<double>("dist_to_travel"), 0.15);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("speed"), 0.025);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("time_allowance"), 10.0);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), false);
 
   xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <DriveOnHeading dist_to_travel="2" speed="0.26" />
+            <DriveOnHeading dist_to_travel="2" speed="0.26" disable_collision_checks="true" />
         </BehaviorTree>
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   EXPECT_EQ(tree_->rootNode()->getInput<double>("dist_to_travel"), 2.0);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("speed"), 0.26);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), true);
 }
 
 TEST_F(DriveOnHeadingActionTestFixture, test_tick)