Select behavior Tree in NavigateToPoseAction

[fmrico]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1780
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation of Tally

---

Description of contribution in a few bullet points

• Changes in NavigateToPose.action to include a field string behavior_tree with the ID of the BT to use. If it is void (""), the use the first BT in params. This change solves backward compatibility.

• Use a list of BTs, in the similar way as plugins (the first could is the BT by default):

    bt_xml_filenames: ["navigate_w_replanning_and_recovery.xml", "navigate_w_replanning_time.xml"]
    bt_xml_ids:  ["default", "time"]

• In bt_navigator tree_ is changed to std::shared_ptr<BT::Tree> to ensure a clean BT when it is changed. Measured BT recreating time. It is inexpensive.

• Remove bt_xml_file param in launchers, as it is selected in by params. Specifying this from two places (in nav2_params.yaml also) could be confusing.

Description of documentation updates required from your changes

• Description of NavigateToPose.action. Field behavior_tree in the request should be described
• Remove all references in launchers to select BT

---

Future work that may be required in bullet points

[codecov]

Codecov Report

Merging #1784 into master will decrease coverage by 0.23%.
The diff coverage is 70.00%.

@@            Coverage Diff             @@
##           master    #1784      +/-   ##
==========================================
- Coverage   66.79%   66.56%   -0.24%     
==========================================
  Files         200      200              
  Lines       10532    10543      +11     
==========================================
- Hits         7035     7018      -17     
- Misses       3497     3525      +28     

Flag	Coverage Δ
#project	66.56% <70.00%> (-0.24%)	⬇️

Impacted Files	Coverage Δ
nav2_bt_navigator/src/bt_navigator.cpp	80.00% <70.00%> (-1.95%)	⬇️
nav2_map_server/src/map_server/main.cpp	63.63% <0.00%> (-36.37%)	⬇️
...tree/include/nav2_behavior_tree/bt_action_node.hpp	68.29% <0.00%> (-7.32%)	⬇️
...v2_util/include/nav2_util/simple_action_server.hpp	85.80% <0.00%> (-4.94%)	⬇️
...lifecycle_manager/src/lifecycle_manager_client.cpp	48.75% <0.00%> (-1.25%)	⬇️
nav2_amcl/src/amcl_node.cpp	83.38% <0.00%> (-0.61%)	⬇️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 9e304b1...8cdf085. Read the comment docs.

[gramss]

Wouldn't it be more easier to implement to do this behavior through a parameter server?

You could also have like multiple input vectors..:

• via launch (just one regular BT, easy to use for those that don't care to change behaviors)
• when a new action request comes the param server gets checked for existens and compares the current entry with the previous BT_id / filename

I dislike the idea to have it in the action, as I would personally like to have them clean. In the end you could argue to include nearly all params through the action request. I think that is exactly why we have parameter server in ROS2.

Speaking of parameter servers ( I only saw them in design docs and really liked the concept. I did not test them so far and I don't know if nav2 already utilizes them)

Besides that I still like your implementation. You could easily shuffle the internals of the nav2 BT around with a single different param (wherever it might come from.. 🙂 )

[fmrico]

Hi @gramss

@gavanderhoorn, a ROS2 developer that I respect, gave me an explanation last week, in the context of our project MROS, that convinced me about the usage in ROS2 what was called parameter server in ROS1. Actually, I worked here in a way to change Nav2 BT during navigation using parameter server. The explanation was:

"The idea behind introducing the component runtime model was that it makes the state of a ROS application introspectable, predictable and manageable.

This would include being able to control when a node would take new configuration into account. Compared to ROS 1, where dynamic_reconfigure was/is used for this, being able to predict when configuration changes will take effect (and also rely on that behavior) is beneficial.

Node authors could, of course, still either poll their parameters or register for updates outside of the lifecycle, but it would have to be a deliberate choice (and a violation of contract almost I believe).

In ROS 1 there is no telling what a node does upon changes to parameters. That is not a situation we want back in ROS 2."

This reasoning makes me think that it is not good to change the behavior of Nav2 during the execution of navigation actions using parameters. I don't like the idea of including every setting in the action request. Many of them are very related to the characteristics of the robot and shouldn't be specified in every call. But BTs in Nav2 are designed to adapt navigation behavior to the Nav2 user (who call the action) needs. Why should this behavior be the same in every navigation action execution? You can face applications as described in #1780.

Using this approach, you have a default behavior for a particular navigation action, and a way to specify another "special" behavior if needed.

[SteveMacenski]

See questions in ticket, we still should cover those open questions first, and I dont think it would be wise to fix this up until we have answers we can all agree upon.

A bit of my concern is around parameterization and configuration management. E.g. if we even have parameters for behavior trees mapped or the requester gives a file path and there's no mapped names.

[fmrico]

Hi all,

I have changed the code to specify the full path to the behavior tree to use in the navigation. The default behavior tree is specified by the default_bt_xml_filename parameter. As this parameter is a full path of a BT XML file, it's mandatory to specify in launchers (as currently happens).

@SteveMacenski, after 11 executions specifying different BT files, including default, the time spent to change the BT is 24,56 milliseconds, with a standard deviation of 7,67 ms. I think it is assumable. Of course, nothing is spent if the BT is the same as the last navigation action execution.

Best

[fmrico]

Wait!! I did a rebase and I am including commits not related to my PR... I will try to fix it

[fmrico]

Hi @SteveMacenski

Now it's fixed. I have force-pushed only with my commits. You can now review

Francisco

[SteveMacenski]

A couple of logic updates but I think its on the right track. I would spend a little more time seeing if you can get it to work without the shared / smart pointer. That will contribute to the latency of creating the tree and I want to absolutely avoid that if possible.

[SteveMacenski]

Can you test to show that this does what you think it should?

[SteveMacenski]

@fmrico what's the latency for tree creation now that its not a shared pointer?

[SteveMacenski]

Also you're changing parameter names. Please update https://github.com/ros-planning/navigation2/blob/master/doc/parameters/param_list.md and https://github.com/ros-planning/navigation.ros.org/blob/master/configuration/packages/configuring-the-bt.rst params that you added/subtracted and new description

[fmrico]

Hi @SteveMacenski

PRs to docs done.

After 14 executions with different BTs, the loadBehaviorTree, when changes BT, takes 24,58 ms in mean, with 6,5 of stddev.

[gramss]

@fmrico

Thank you for your in-depth explanation about the usage of parameter server.

I strongly share your opinion about not changing the behavior during the execution of the action / behavior.

I was only pitching the idea of using the parameter server for delivering the path for the „next“ BT.

So every time the action would be called, the parameter server gets checked like your initial action message. And only here it has the power to change the BT before the BT gets build (like in your approach)

So while changing this parameter while executing a BT action, this would have no affect to the current BT. I would only check it right after the action goal is received and before the BT is rebuild anyway.

But my point might be already to late in the discussion. Would love to hear your standpoint about that. I think it shifts a little bit the usage of a parameter server that you first thought of.
And might minimize the impact on the API, while being a bit more hidden to take use of it. But a good documentation (from me in this case) could help that. :)

[SteveMacenski]

@gramss can you be concise in what your suggested change or approval is for? I don't see a clear reference to something you either want changed in this PR or something you like.

@fmrico only blocker is the updating to failing if BT isn't valid & testing that this works as intended with your application.

[gramss]

Hey @SteveMacenski , yes sorry. I should have quote replied. This is answering this comment: #1784 (comment)

It seems to have been collapsed after i posted my comment.

My idea was to not set the BT to execute via an additional field in the action request to Nav2 but rather use a parameter server entry. (You can jump to TL;DR here..)

Potential benefit: this could make this additional feature of changing the BT more inclusive?

Thinking this through:
Existing nodes calling nav2 would not need to change/update the action msg for nav2 in their node.
But I guess the impact on other nodes would be minimal as this field is optional? So existing nodes calling nav2 via this action „API“ probably won‘t notice an additional available field they can write too. Just rebuild the node with the new action message they include from the updated nav2 package and their update to this would be finished?
Adding the overhead of including interacting with an parameter server from these nodes might be bad on two sides.
The first would be that they need to integrate a big „overhead“ inside their code to learn / work with parameter servers in ROS2.
The second issue with this idea is, that you cannot be sure what BT is currently set, if their are two nodes calling the action server simultaneously.

TL;DR:
I recall my idea. I thought that parameter servers would be a good fit instead of an additional field in the action message request.
Was just elaborating over „new“ ROS2 features and potential new recommenced ways to interact with different action servers.

[fmrico]

Hi @gramss

I don't quite understand your proposal. I think we agree that using parameter servers in lifecycle nodes is not convenient because they have just been designed so that the configuration should be done in the Unconfigured -> Inactive transition.

I understand that you propose that if a node (the one which defines the robot behavior) wants to specify the BT used to navigate, it must first change the parameter in bt_xml_file in bt_navigator, and then call the NavigateToPose action, Right? If so:

• How do you control that the parameter is changed, but the call to action is not made? How does this affect subsequent calls to NavigateToPose?
• As you have already introduced, how can a node know which BT will be used in navigation? Should you always modify the parameter to make sure the one you expect is used? Or querying to bt_navigatior before every call?
• How are race conditions handled in a situation where more than one node wants to call NavigateToPose, each one modifying bt_xml_file parameter?

I propose to continue with this PR, which is practically finished, and wait for another PR with your proposal so that we can see the answers to these questions. I think we could create a new issue (or use #1780 ) to see the pros and cons.

Thank you for your contribution to this discussion.

[SteveMacenski]

Great new feature, thanks Francisco!

[fmrico]

Thanks, @SteveMacenski !! 😊

[tonynajjar]

Hi, as a result of this PR, would it not make sense for the navigate_to_pose_action.cpp (BT action node) to have the option to set the behavior tree?
I'm thinking of doing this in my project and was wondering if it was an overlook or by choice. I can contribute it if needed. Thanks!

[SteveMacenski]

I don't understand the comment

[tonynajjar]

The NavigateToPose Action server accepts a behavior tree as input but its corresponding BT action node does not have a behavior_tree option. My question is whether we should add it. I hope it's clearer?

[SteveMacenski]

I see, looks like an oversight. I'd be more than happy to merge a PR implementing the full API on the BT node