Skip to content

Commit

Permalink
Fix bug in nav2_behavior_tree/bt_action_node (#3849)
Browse files Browse the repository at this point in the history 
* 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>
  • Loading branch information
@CihatAltiparmak @SteveMacenski
CihatAltiparmak authored and SteveMacenski committed Jan 24, 2024
1 parent 945a18d commit 791aa90
Show file tree
Hide file tree
Showing 2 changed files with 143 additions and 1 deletion.
11 changes: 11 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -299,6 +299,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)
Expand All @@ -307,6 +308,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);
Expand Down
133 changes: 132 additions & 1 deletion nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
}
Expand All @@ -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) {
Expand All @@ -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);
Expand All @@ -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>
Expand All @@ -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")});
Expand All @@ -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)
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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);
Expand Down

0 comments on commit 791aa90

Please sign in to comment.