Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

bt_service_node and bt_action_node: Don't block BT loop (backport #4214) #4408

Merged
merged 2 commits into from
Jun 24, 2024
Merged

bt_service_node and bt_action_node: Don't block BT loop (backport #4214) #4408

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7d7d462
bt_service_node and bt_action_node: Don't block BT loop (#4214)
christophfroehlich Jun 24, 2024
2ed5d0d
Increasing test count from timeout handling changes (#4234)
SteveMacenski Jun 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 6 additions & 3 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,14 +56,17 @@ class BtActionNode : public BT::ActionNodeBase
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());

// Get the required items from the blackboard
bt_loop_duration_ =
auto bt_loop_duration =
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");

// timeout should be less than bt_loop_duration to be able to finish the current tick
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);

// Initialize the input and output messages
goal_ = typename ActionT::Goal();
result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
Expand Down Expand Up @@ -405,7 +408,7 @@ class BtActionNode : public BT::ActionNodeBase
return false;
}

auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;
auto result =
callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
elapsed += timeout;
Expand Down Expand Up @@ -461,7 +464,7 @@ class BtActionNode : public BT::ActionNodeBase
std::chrono::milliseconds server_timeout_;

// The timeout value for BT loop execution
std::chrono::milliseconds bt_loop_duration_;
std::chrono::milliseconds max_timeout_;

// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;
Expand Down
9 changes: 6 additions & 3 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,14 +57,17 @@ class BtServiceNode : public BT::ActionNodeBase
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());

// Get the required items from the blackboard
bt_loop_duration_ =
auto bt_loop_duration =
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");

// timeout should be less than bt_loop_duration to be able to finish the current tick
max_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(bt_loop_duration * 0.5);

// Now that we have node_ to use, create the service client for this BT service
getInput("service_name", service_name_);
service_client_ = node_->create_client<ServiceT>(
Expand Down Expand Up @@ -189,7 +192,7 @@ class BtServiceNode : public BT::ActionNodeBase
auto remaining = server_timeout_ - elapsed;

if (remaining > std::chrono::milliseconds(0)) {
auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining;

rclcpp::FutureReturnCode rc;
rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout);
Expand Down Expand Up @@ -249,7 +252,7 @@ class BtServiceNode : public BT::ActionNodeBase
std::chrono::milliseconds server_timeout_;

// The timeout value for BT loop execution
std::chrono::milliseconds bt_loop_duration_;
std::chrono::milliseconds max_timeout_;

// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;
Expand Down
8 changes: 5 additions & 3 deletions nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -316,8 +316,10 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success)
// the BT should have failed
EXPECT_EQ(result, BT::NodeStatus::FAILURE);

// since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2
EXPECT_EQ(ticks, 2);
// since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should
// be at most 2, but it can be 1 too, because the tickOnce may execute two ticks.
EXPECT_LE(ticks, 3);
EXPECT_GE(ticks, 1);
}

TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
Expand Down Expand Up @@ -362,7 +364,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure)
EXPECT_EQ(result, BT::NodeStatus::FAILURE);

// since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9
EXPECT_EQ(ticks, 9);
EXPECT_EQ(ticks, 10);

// start a new execution cycle with the previous BT to ensure previous state doesn't leak into
// the new cycle
Expand Down