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

Refactor of 2624 for simplicity #2628

Closed
wants to merge 5 commits into from
Closed

Refactor of 2624 for simplicity #2628

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
584adb0
addig try/catch around non-tree exceptions to fail node
SteveMacenski Oct 18, 2021
24c67eb
Update bt_action_node.hpp
SteveMacenski Oct 19, 2021
b38a21e
linting
SteveMacenski Oct 19, 2021
1b18f6c
Merge branch 'hi_2376' of github.com:ros-planning/navigation2 into hi…
SteveMacenski Oct 19, 2021
966b2e9
refactoring #2624 for simplicity
SteveMacenski Oct 19, 2021
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
82 changes: 47 additions & 35 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 @@ -183,41 +183,17 @@ class BtActionNode : public BT::ActionNodeBase
send_new_goal();
}

// if new goal was sent and action server has not yet responded
// check the future goal handle
if (future_goal_handle_) {
auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
// return RUNNING if there is still some time before timeout happens
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
}
// if server has taken more time to respond than the specified timeout value return FAILURE
RCLCPP_WARN(
node_->get_logger(),
"Timed out while waiting for action server to acknowledge goal request for %s",
action_name_.c_str());
future_goal_handle_.reset();
return BT::NodeStatus::FAILURE;
}
}

// The following code corresponds to the "RUNNING" loop
if (rclcpp::ok() && !goal_result_available_) {
// user defined callback. May modify the value of "goal_updated_"
on_wait_for_result();

auto goal_status = goal_handle_->get_status();
if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
{
goal_updated_ = false;
send_new_goal();
try {
// if new goal was sent and action server has not yet responded
// check the future goal handle
if (future_goal_handle_) {
auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
// return RUNNING if there is still some time before timeout happens
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
}
// if server has taken more time than the specified timeout value return FAILURE
RCLCPP_WARN(
node_->get_logger(),
"Timed out while waiting for action server to acknowledge goal request for %s",
Expand All @@ -227,12 +203,48 @@ class BtActionNode : public BT::ActionNodeBase
}
}

callback_group_executor_.spin_some();
// The following code corresponds to the "RUNNING" loop
if (rclcpp::ok() && !goal_result_available_) {
// user defined callback. May modify the value of "goal_updated_"
on_wait_for_result();

auto goal_status = goal_handle_->get_status();
if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
{
goal_updated_ = false;
send_new_goal();
auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
if (!is_future_goal_handle_complete(elapsed)) {
if (elapsed < server_timeout_) {
return BT::NodeStatus::RUNNING;
}
RCLCPP_WARN(
node_->get_logger(),
"Timed out while waiting for action server to acknowledge goal request for %s",
action_name_.c_str());
future_goal_handle_.reset();
return BT::NodeStatus::FAILURE;
}
}

// check if, after invoking spin_some(), we finally received the result
if (!goal_result_available_) {
// Yield this Action, returning RUNNING
return BT::NodeStatus::RUNNING;
callback_group_executor_.spin_some();

// check if, after invoking spin_some(), we finally received the result
if (!goal_result_available_) {
// Yield this Action, returning RUNNING
return BT::NodeStatus::RUNNING;
}
}
} catch (const std::runtime_error & e) {
if (e.what() == std::string("send_goal failed") ||
e.what() == std::string("Goal was rejected by the action server"))
{
// Action related failure that should not fail the tree, but the node
return BT::NodeStatus::FAILURE;
} else {
// Internal exception to propagate to the tree
throw e;
}
}

Expand Down