Fix bug in nav2_behavior_tree/bt_action_node (ros-navigation#3849)

* Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>
floatic-unicorn · Oct 2, 2023 · bfb4506 · bfb4506
1 parent c671d5e
commit bfb4506
Show file tree

Hide file tree

Showing 2 changed files with 143 additions and 1 deletion.
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -302,6 +302,7 @@ class BtActionNode : public BT::ActionNodeBase
   void halt() override
   {
     if (should_cancel_goal()) {
+      auto future_result = action_client_->async_get_result(goal_handle_);
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
       if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
         rclcpp::FutureReturnCode::SUCCESS)
@@ -310,6 +311,16 @@ class BtActionNode : public BT::ActionNodeBase
           node_->get_logger(),
           "Failed to cancel action server for %s", action_name_.c_str());
       }
+
+      if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) !=
+        rclcpp::FutureReturnCode::SUCCESS)
+      {
+        RCLCPP_ERROR(
+          node_->get_logger(),
+          "Failed to get result for %s in node halt!", action_name_.c_str());
+      }
+
+      on_cancelled();
     }
 
     setStatus(BT::NodeStatus::IDLE);

diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
@@ -54,11 +54,17 @@ class FibonacciActionServer : public rclcpp::Node
     sleep_duration_ = sleep_duration;
   }
 
+  void setServerLoopRate(std::chrono::nanoseconds server_loop_rate)
+  {
+    server_loop_rate_ = server_loop_rate;
+  }
+
 protected:
   rclcpp_action::GoalResponse handle_goal(
     const rclcpp_action::GoalUUID &,
     std::shared_ptr<const test_msgs::action::Fibonacci::Goal>)
   {
+    RCLCPP_INFO(this->get_logger(), "Goal is received..");
     if (sleep_duration_ > 0ms) {
       std::this_thread::sleep_for(sleep_duration_);
     }
@@ -73,6 +79,13 @@ class FibonacciActionServer : public rclcpp::Node
 
   void handle_accepted(
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> handle)
+  {
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    std::thread{std::bind(&FibonacciActionServer::execute, this, _1), handle}.detach();
+  }
+
+  void execute(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> handle)
   {
     // this needs to return quickly to avoid blocking the executor, so spin up a new thread
     if (handle) {
@@ -88,8 +101,17 @@ class FibonacciActionServer : public rclcpp::Node
       sequence.push_back(0);
       sequence.push_back(1);
 
+      rclcpp::Rate rate(server_loop_rate_);
       for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
+        if (handle->is_canceling()) {
+          RCLCPP_INFO(this->get_logger(), "Goal is canceling.");
+          handle->canceled(result);
+          return;
+        }
+
+        RCLCPP_INFO(this->get_logger(), "Goal is feedbacking.");
         sequence.push_back(sequence[i] + sequence[i - 1]);
+        rate.sleep();
       }
 
       handle->succeed(result);
@@ -99,6 +121,7 @@ class FibonacciActionServer : public rclcpp::Node
 protected:
   rclcpp_action::Server<test_msgs::action::Fibonacci>::SharedPtr action_server_;
   std::chrono::milliseconds sleep_duration_;
+  std::chrono::nanoseconds server_loop_rate_;
 };
 
 class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::action::Fibonacci>
@@ -121,6 +144,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::actio
     return BT::NodeStatus::SUCCESS;
   }
 
+  BT::NodeStatus on_cancelled() override
+  {
+    config().blackboard->set<std::vector<int>>("sequence", result_.result->sequence);
+    config().blackboard->set<bool>("on_cancelled_triggered", true);
+    return BT::NodeStatus::SUCCESS;
+  }
+
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts({BT::InputPort<int>("order", "Fibonacci order")});
@@ -144,6 +174,7 @@ class BTActionNodeTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 20ms);
     config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
     config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<bool>("on_cancelled_triggered", false);
 
     BT::NodeBuilder builder =
       [](const std::string & name, const BT::NodeConfiguration & config)
@@ -220,6 +251,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success)
 
   // setting a small action server goal handling duration
   action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(10ns);
 
   // to keep track of the number of ticks it took to reach a terminal result
   int ticks = 0;
@@ -255,15 +287,22 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success)
   // start a new execution cycle with the previous BT to ensure previous state doesn't leak into
   // the new cycle
 
-  // halt BT for a new execution cycle
+  // halt BT for a new execution cycle,
+  // get if the on_cancelled is triggered from blackboard and assert
+  // that the on_cancelled triggers after halting node
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
   tree_->haltTree();
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, false);
 
   // setting a large action server goal handling duration
   action_server_->setHandleGoalSleepDuration(100ms);
+  action_server_->setServerLoopRate(10ns);
 
   // reset state variables
   ticks = 0;
   result = BT::NodeStatus::RUNNING;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
 
   // main BT execution loop
   while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
@@ -300,6 +339,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
 
   // the action server will take 100ms before accepting the goal
   action_server_->setHandleGoalSleepDuration(100ms);
+  action_server_->setServerLoopRate(10ns);
 
   // to keep track of the number of ticks it took to reach a terminal result
   int ticks = 0;
@@ -327,14 +367,21 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
   // the new cycle
 
   // halt BT for a new execution cycle
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancelled never can trigger after halting node
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
   tree_->haltTree();
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, false);
 
   // setting a small action server goal handling duration
   action_server_->setHandleGoalSleepDuration(25ms);
+  action_server_->setServerLoopRate(10ns);
 
   // reset state variables
   ticks = 0;
   result = BT::NodeStatus::RUNNING;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
 
   // main BT execution loop
   while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
@@ -348,6 +395,90 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
   EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
 }
 
+TEST_F(BTActionNodeTestFixture, test_server_cancel)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Fibonacci order="1000000" />
+        </BehaviorTree>
+      </root>)";
+
+  // setting a server timeout smaller than the time the action server will take to accept the goal
+  // to simulate a server timeout scenario
+  config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 100ms);
+  config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // the action server will take 2ms before accepting the goal
+  // and the feedback period of the action server will be 50ms
+  action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(50ms);
+
+  // to keep track of the number of ticks it took to reach expected tick count
+  int ticks = 0;
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  // BT loop execution rate
+  rclcpp::WallRate loopRate(100ms);
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // halt BT for testing if the action node cancels the goal correctly
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
+  tree_->haltTree();
+
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancel is triggered after halting node
+  bool on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, true);
+
+  // ticks variable must be 5 because execution time of the action server
+  // is at least 1000000 x 50 ms
+  EXPECT_EQ(ticks, 5);
+
+  // send new goal to the action server for a new execution cycle
+
+  // the action server will take 2ms before accepting the goal
+  // and the feedback period of the action server will be 1000ms
+  action_server_->setHandleGoalSleepDuration(2ms);
+  action_server_->setServerLoopRate(50ms);
+
+  // reset state variable
+  ticks = 0;
+  config_->blackboard->set<bool>("on_cancelled_triggered", false);
+  result = BT::NodeStatus::RUNNING;
+
+  // main BT execution loop
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) {
+    result = tree_->tickRoot();
+    ticks++;
+    loopRate.sleep();
+  }
+
+  // halt BT for testing if the action node cancels the goal correctly
+  RCLCPP_INFO(node_->get_logger(), "Tree is halting.");
+  tree_->haltTree();
+
+  // get if the on_cancel is triggered from blackboard and assert
+  // that the on_cancel is triggered after halting node
+  on_cancelled_triggered = config_->blackboard->get<bool>("on_cancelled_triggered");
+  EXPECT_EQ(on_cancelled_triggered, true);
+
+  // ticks variable must be 7 because execution time of the action server
+  // is at least 1000000 x 50 ms
+  EXPECT_EQ(ticks, 7);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);