Skip to content

Check if goal_status_ is valid. #2305

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

Closed
wants to merge 6 commits into from
Closed

Check if goal_status_ is valid. #2305

Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e95a229
Check if goal_status_ is valid.
tzskp1 Apr 8, 2021
d77e5b9
Get rid of unnecessarily check.
tzskp1 Apr 13, 2021
09d2ea9
Revert "Get rid of unnecessarily check."
tzskp1 Apr 13, 2021
cdcebf9
Reset goal_handle_ due to validation.
tzskp1 Apr 16, 2021
7877a12
guard condition of goal_handle_
tzskp1 Apr 16, 2021
d7beb71
Care of first tick
tzskp1 Apr 17, 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
14 changes: 12 additions & 2 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 @@ -177,6 +177,10 @@ class BtActionNode : public BT::ActionNodeBase
on_new_goal_received();
}

if (!goal_handle_) {
throw std::logic_error("BtActionNode::Tick: invalid goal handle");
Copy link
Member

@SteveMacenski SteveMacenski Apr 16, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure.... that would make it exit in the condition that the goal_handle_ is already complete, but lets think about this more strategically -- because otherwise your navigation system every time you run into this issue is going to be throwing this error all over the place.

What situations do you have that goal_handle_ is not valid? Answer that main question first. Is it only when canceled or aborted? Where is it happening? If so, then in the result callback, you can check if its not valid and then have an else statement to set the result type to that return code https://github.com/ros-planning/navigation2/blob/7877a1225c549aa0ced78a0f263cbff31829761c/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L276-L279. Then since result_ has a populated value, you get a clean exit on the next iteration.

Rather than masking this issue, we should work to fix it. When / why is goal_handle_ being turned into a nullptr? Answer that, and we can find a real solution -- whether that's fixing an issue in this file, or in the file that's nulling out the pointer

Copy link
Author

@tzskp1 tzskp1 Apr 17, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok. I will start investigation.

Copy link
Author

@tzskp1 tzskp1 Apr 17, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

    send_goal_options.result_callback =
      [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
        // TODO(#1652): a work around until rcl_action interface is updated
        // if goal ids are not matched, the older goal call this callback so ignore the result
        // if matched, it must be processed (including aborted)
        if (!this->goal_handle_) {
          RCLCPP_ERROR(node_->get_logger(), "error id: %d", result.code);
        }
        if (this->goal_handle_ && (this->goal_handle_->get_goal_id() == result.goal_id)) {
          goal_result_available_ = true;
          result_ = result;
        }
      };

[bt_navigator-11] [ERROR] [1618637514.271798607] [bt_navigator_rclcpp_node]: error id: 4
[bt_navigator-11] [ERROR] [1618637514.272602967] [bt_navigator_rclcpp_node]: error id: 5
[bt_navigator-11] [ERROR] [1618637647.774230873] [bt_navigator_rclcpp_node]: error id: 4
[bt_navigator-11] [ERROR] [1618637647.839067741] [bt_navigator_rclcpp_node]: error id: 4
[bt_navigator-11] [ERROR] [1618637698.121035886] [bt_navigator_rclcpp_node]: error id: 5
[bt_navigator-11] [ERROR] [1618637712.920679316] [bt_navigator_rclcpp_node]: error id: 5

According to experiment, result.code is 4 or 5(namely SUCCEEDED or CANCELED).

Copy link
Author

@tzskp1 tzskp1 Apr 21, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What situations do you have that goal_handle_ is not valid?

It is when on_new_goal_received is not called yet.

When / why is goal_handle_ being turned into a nullptr?

As far as I can see, goal_handle_ is not turned into a nullptr.
goal_handle_ is originally a nullptr when BT node is created.
But original code made assumption that goal_handle_ is always valid.

Copy link
Member

@SteveMacenski SteveMacenski Apr 26, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@tzskp1 https://github.com/ros-planning/navigation2/blob/d7beb7104b952f89399fac7b78b80074a7d55ddc/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L177 on_new_goal_received is the first function called after on_tick() which populates that goal_ on the first iteration.

It is incorrect that we always assume its valid, but we need to then reset this handle properly when its terminated and also know what a nullpointer means in the context. The goal_handle_ inside of the lambda in on_new_goal_received isn't called until the result message is received (its another function, not code called on the first call of on_new_goal_received)

Copy link
Author

@tzskp1 tzskp1 Apr 27, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

its another function, not code called on the first call of on_new_goal_received

As far as I know, this spin calls the callback.
https://github.com/ros-planning/navigation2/blob/7877a1225c549aa0ced78a0f263cbff31829761c/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L284
So the assignment statement follows the callback.

My intention is that nullptr should be one of the following.

  • BT node is created. (goal_handle_ is initialized to nullptr by constructor.)
  • BT node is halted.

}

// The following code corresponds to the "RUNNING" loop
if (rclcpp::ok() && !goal_result_available_) {
// user defined callback. May modify the value of "goal_updated_"
Expand Down Expand Up @@ -232,6 +236,9 @@ class BtActionNode : public BT::ActionNodeBase
}

setStatus(BT::NodeStatus::IDLE);
if (goal_handle_) {
goal_handle_.reset();
}
}

protected:
Expand All @@ -242,7 +249,7 @@ class BtActionNode : public BT::ActionNodeBase
bool should_cancel_goal()
{
// Shut the node down if it is currently running
if (status() != BT::NodeStatus::RUNNING) {
if (status() != BT::NodeStatus::RUNNING || !goal_handle_) {
return false;
}

Expand All @@ -266,7 +273,7 @@ class BtActionNode : public BT::ActionNodeBase
// TODO(#1652): a work around until rcl_action interface is updated
// if goal ids are not matched, the older goal call this callback so ignore the result
// if matched, it must be processed (including aborted)
if (this->goal_handle_->get_goal_id() == result.goal_id) {
if (this->goal_handle_ && (this->goal_handle_->get_goal_id() == result.goal_id)) {
Copy link
Member

@SteveMacenski SteveMacenski Apr 12, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In this case, if this callback is triggered, there must be a result so the goal handle has to be valid still, right? Did you end up actually having an issue in this callback? If not, I think this one can be removed.

If this one does need to stay, then we need to have some condition to check if the goal handle is invalid and if so, think closely about the value we should be setting for goal_result_available_ and result_ in that case. This would create a problem if goal_handle_ was invalid that a result was triggered by this callback but then the BT node's tick() never has a result_ to exit https://github.com/ros-planning/navigation2/blob/e95a2299be32862cd37a25ff5571e3ceed2aaca8/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L204-L212 and move onto other nodes.

Copy link
Author

@tzskp1 tzskp1 Apr 13, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

According to experiment, this check is needed.

goal_result_available_ = true;
result_ = result;
}
Expand All @@ -280,6 +287,9 @@ class BtActionNode : public BT::ActionNodeBase
throw std::runtime_error("send_goal failed");
}

if (goal_handle_) {
goal_handle_.reset();
}
goal_handle_ = future_goal_handle.get();
if (!goal_handle_) {
throw std::runtime_error("Goal was rejected by the action server");
Expand Down