Open AnthonyBreton opened 3 months ago
I think this explains better issue https://github.com/BehaviorTree/BehaviorTree.ROS2/issues/48
please try branch issue_76 and let me know if problem persists
@facontidavide Unfortunately, it does not seem to solve the issue. I still receive a SEND_GOAL_TIMEOUT
error after 10 seconds. I changed the log level of the tree_executor to debug
. Here's the traceback, maybe it can help you:
... 10s later
And my overridden onFailure
method of my BT::RosActionNode<Nav2>
I still have the same behavior where my other BT action node for docking works properly but not the nav2 one. I also tried adding a max_duration
to the spin_some
like I did previously with the timeout
of the spin_until_future_complete
, but it did not work like last time:
client_instance_->callback_executor.spin_some(std::chrono::nanoseconds(10000000));
I need a way to reproduce this. Is there any dummy server and client that can be used to reproduce the error?
It seems to me like a potential error in the server. Also, which version of ROS are you using?
I'm running ROS2 Humble on Ubuntu 22.04
I joined a package with a custom BT with a simple sequence where a condition waits for geometry_msgs::msg::Pose
on a topic to be received then set it to the blackboard where a Nav2_action_client
can take it and send an action goal to the Nav2 action server /navigate_to_pose
.
All you will need is:
Turtlebot3
running example of Nav2 and it works I can reproduce the bug https://docs.nav2.org/getting_started/index.html#running-the-exampleros2 launch dummy_behavior_tree bt.launch.py
you can use the verbose parameter to activate or deactivate the StdCoutLogger
(verbose:=false
)/dummy_goal_pose
with this cmd line ros2 topic pub /dummy_goal_pose geometry_msgs/msg/Pose "position: x: 0.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 " -1
I hope I didn't leave any bugs in the dummy_behavior_tree
package. If I did, let me know, and I'll try to fix them. I can also spend some time helping you solve this bug if you don't have the time. However, I have limited knowledge of the behaviortree.ros2 package, so it might take me longer to find and properly resolve the issue.
@facontidavide, were you able to reproduce the error? I also realized that downloading a zip from a GitHub comment might not be ideal. If you prefer, I can create a public repository with the code instead of the zip file.
@facontidavide @AnthonyBreton the branch issue_76 doesn't seem to fix the bugs on my side either.
I'm working on a fix on this fork: https://github.com/Oscar-Robotics/BehaviorTree.ROS2/tree/fix_issue_76
It is heavily inspired by Nav2's bt_action_node logic. The timeout for the spin_until_future_complete
corresponds to half the time between ticks, so it does not block the execution of the rest of the tree. Right now, the value is hard coded.
The only thing missing before a PR is having a way to know the tick frequency of the tree, probably through the blackboard as Nav2 does:
If anyone knows a simple way of doing this, I would love the feedback. @facontidavide @MarqRazz
Running ROS 2 Humble on Ubuntu 22.04
The problem
I'm using a behavior tree that has a BT action node to send a goal to the Nav2
navigate_to_pose
action server. My problem is that if the robot navigates for more than 10 seconds (which is the default server_timeout of aBT::RosActionNode
), the node'sonFailure
method is triggered and returns the error codeSEND_GOAL_TIMEOUT
. From what I've read of thebt_action_node.hpp
file, this means that the client did not receive a response regarding whether the goal was accepted or not from the server within 10 seconds.The thing is that even if I haven't received a goal_response, the Nav2 action server runs normally and the robot navigates (since
async_send_goal
is used I'm guessing). I think the problem lies in the way thatbt_action_node.hpp
(line 459 and after) checks thefuture
to receive thegoal_handle
. It seems to never returnrclcpp::FutureReturnCode::SUCCESS
in thespin_until_future_complete
with no delay.What's weird is that if the navigation time is under 10 seconds, then the BT action node does not fail, but I can see in the prints that the
goal_response_callback
only gets called after the robot reaches its navigation goal. This means that the response about whether the action goal is accepted or not is only received after the robot is done navigating. I also don't receive any feedback from the action server during the navigation, probably since I have no goal_handle.I also have another BT action node for docking in my behavior tree (where the action server was custom-made), and I don't seem to have a problem with it since I can see in the debug prints that the goal was accepted and I receive feedback. Because of this, I'm not sure if it's a Nav2 problem with the way that the
navigate_to_pose
action server was written or an edge case that BehaviorTree.ROS2 did not consider.Maybe a fix
I found a way to make it work for the
navigate_to_pose
action by setting the nodelay inbt_action_node.hpp
(line 452) to 10ms instead of 0ms:auto nodelay = std::chrono::milliseconds(0);
->auto nodelay = std::chrono::milliseconds(10);
I don't have a clear understanding of the reason why thespin_until_future_complete
has a timeout of 0ms. I guess it's to have a non-blocking behavior.I've recreated the same issue with a simple action client node that calls the
navigate_to_pose
action server and handles the goal accepted response with the same logic thatbt_action_node.hpp
has. I'll leave the.hpp
and.cpp
code if you want to test it..hpp
.cpp