Set BT::NodeStatus based on service result msg (class BtServiceNode)

[philison]

Required Info:

• Operating System:
  • Ubuntu 20.04.2 LTS
• ROS2 Version:
  • Galactic binaries
• Version or commit hash:
  • 5a70275
• DDS implementation:

Feature request

Feature description

It is the goal to set the BT::NodeStatus of a behavior tree leave node based on the result send from the service server.
More specifically, I tried to implement a Condition-Node that makes a request to a service server via the ROS2 service interface and sets its BT::NodeStatus based on the result.. Therefore, the BtServiceNode class (bt_service_node.cpp) seemed to me to be the best suited class to use. However, in contrast to the BtActionNode class (bt_action_node.cpp) it missed the virtual on_success() function, that can be used to define the custom BT::NodeStatus behavior based on the service's answer. The current BtServiceNode class sets the BT::NodeStatus solely based on the success of the service request.

I am not sure if I missed a class that implements this feature, if there is a simpler solution or if I should just use the BT::ConditionNode base class with a normal service client after all.

Implementation considerations

The proposed implementation simply adds a call to the virtual on_success() function in the check_future() function (Similar to the BtActionNode).

The changes do not change do not affect the default behavior of the BtServiceNode as the default definition of on_success() does still simply return BT::NodeStatus::SUCCESS.

If the proposed implementation is an appropriate modification I could create a pull request.

Changes:
https://github.com/angsa-robotics/navigation2/blob/033cdc49b4c106f2747772f2081fca47bee6e9ec/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp#L150-L187

Original:

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

Lines 145 to 171 in 0764e5a

	/**
	* @brief Check the future and decide the status of BT
	* @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise
	*/
	virtual BT::NodeStatus check_future()
	{
	auto elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
	auto remaining = server_timeout_ - elapsed;
	if (remaining > std::chrono::milliseconds(0)) {
	auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
	rclcpp::FutureReturnCode rc;
	rc = callback_group_executor_.spin_until_future_complete(future_result_, server_timeout_);
	if (rc == rclcpp::FutureReturnCode::SUCCESS) {
	request_sent_ = false;
	return BT::NodeStatus::SUCCESS;
	}
	if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
	on_wait_for_result();
	elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
	if (elapsed < server_timeout_) {
	return BT::NodeStatus::RUNNING;
	}
	}
	}

Edit: Updated BtServiceNode Link to no longer point to the changed/forked version.

[SteveMacenski]

The current BtServiceNode class sets the BT::NodeStatus solely based on the success of the service request.

Yes, but its virtual, if you made your own custom BTServiceNode implementation using it, you can override it with whatever you like. This is exactly the same as the action node. Ex.

navigation2/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp

Lines 41 to 46 in c6294d5

	BT::NodeStatus ComputePathThroughPosesAction::on_success()
	{
	setOutput("path", result_.result->path);
	return BT::NodeStatus::SUCCESS;
	}

Edit: you linked to your fork / changed, no the core tools, that mixed me up.

Why don't you just submit a draft PR and we can take a look together so that we have some diffs to review. What you describe sounds reasonable. The only thing I'd probably ask you to do is make it more complete, so that if there are any other on_XYZ()s we're missing, adding those too. success might not be the right word for what you're describing though, complete might be since success already implies well... success.

[philison]

Sorry for the confusion. I thought I had double checked, so exactly that wouldn't happen, but it is the wrong link.
Yes exactly, I was using the BtActionNode as an inspiration. There, a differentiation is made between all of the possible request results of the action interface (each with the corresponding callback).

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

Lines 239 to 255 in c417e2f

	BT::NodeStatus status;
	switch (result_.code) {
	case rclcpp_action::ResultCode::SUCCEEDED:
	status = on_success();
	break;
	case rclcpp_action::ResultCode::ABORTED:
	status = on_aborted();
	break;
	case rclcpp_action::ResultCode::CANCELED:
	status = on_cancelled();
	break;
	default:
	throw std::logic_error("BtActionNode::Tick: invalid status value");
	}

However, as cancelling and aborting is not possible with services, those would not be relevant.
An on-function that might be relevant would be the on_wait_for_result(), as implemented in the BtActionNode.

Edit: Never mind, the on_wait_for_result() functionality does already exist.

navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

Lines 180 to 186 in c417e2f

	/**
	* @brief Function to perform some user-defined operation after a timeout waiting
	* for a result that hasn't been received yet
	*/
	virtual void on_wait_for_result()
	{
	}

Regarding the wording, are you alluding to the two different types of success ?

1. the success that refers to whether the ros2 interface interaction/communication was successful (more of a complete ?)
2. the success that refers to whether the actual task, that was triggered by calling the service, was successful

I agree with you about the wording issue, as the on_success() name refers to the rclcpp::FutureReturnCode::SUCCESS of the service call (case 1.), but does not necessarily mean that the BtNodeStatus is also a success. But I think it is used in the same context in the BtActionNode.

Thanks.

[SteveMacenski]

I think if you open a Pr, this would be much easier so we're talking over specific code vs abstract ideas (especially since you already did it)

[philison]

I finally got to open a Pr #2481. I had to do some commit/feature de-cluttering, therefore it took a while.

[SteveMacenski]

Merging imminent