Add DistanceController decorator node

[naiveHobo]

Signed-off-by: Sarthak Mittal sarthakmittal2608@gmail.com

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1692
Primary OS tested on	Ubuntu 18.04
Robotic platform tested on	TurtleBot3 gazebo simulation

---

Description of contribution in a few bullet points

• Added DistanceController decorator node to replan after every N meters

Description of documentation updates required from your changes

• Add DistanceController in nav2_bt_navigator/README.md

[SteveMacenski]

Some unit test here would be good. Load this node into your test object, send some TF and make sure it triggers after N updates of M meters.

Add to nav2_behavior_tree readme table, nav2_bt_navigator description, https://navigation.ros.org/plugins/index.html, and https://navigation.ros.org/migration/Eloquent.html. I began discussing what kind of documentation we might need for the BT on the website in #1698 with @maxlein, I think talking about to design trees and this node vs the time node would be a good topic. If you had any suggestions for what types of docs could help behavior tree designers or behavior tree node developers, that would also be great to discuss to get a list going.

---

Ref BehaviorTree/BehaviorTree.CPP#106 (comment)

Can you look at the BT that Davide proposed in this comment. It would be great to reformat the BT to use this node in that way. What I'm looking for is the ability to have a replan on new goal. Right now with just the rate controller, if the rate is set at 0.001Hz, then if a preemption comes in, it'll still wait 1000s before replanning. Adding a BT condition node for if there's a new goal on the blackboard to replan with would then make it immediate regardless of the rate controller (or this distance). I would ignore the suggestion to have the rate controller and the distance controller in the same behavior tree unless you had an idea for how to combine that information. E.g. is it either the rate OR distance, is it rate AND distance, etc to trigger a replan. Obviously the new goal would immediately need to replan. Lets start with just the replan node without adding both in.

You can use this script to generate a picture of the tree https://github.com/ros-planning/navigation2/blob/master/tools/bt2img.py (also you need to add your new node to the list)

[SteveMacenski]

I think its just documentation and we're good to go. See comments above on website / changes / showing image of the BT.

Have you tested this out in simulation to make sure it works? This isn't covered by any good tests. We should definitely change that, but not sure that's in the scope of this ticket, beyond maybe some unit tests of just this individual node. That could be a useful foundation for us to have other folks (or me) use that as the basis for unit tests of the other BT nodes.

[naiveHobo]

Have you tested this out in simulation to make sure it works? This isn't covered by any good tests. We should definitely change that, but not sure that's in the scope of this ticket, beyond maybe some unit tests of just this individual node. That could be a useful foundation for us to have other folks (or me) use that as the basis for unit tests of the other BT nodes.

I have added some tests for the distance controller. I think this structure should be good enough to write unit tests for the other plugins as well. I had to add a header file for the distance controller plugin to make it available to the tests, I think we'll have to do the same for the other plugins.

What I'm looking for is the ability to have a replan on new goal. Right now with just the rate controller, if the rate is set at 0.001Hz, then if a preemption comes in, it'll still wait 1000s before replanning. Adding a BT condition node for if there's a new goal on the blackboard to replan with would then make it immediate regardless of the rate controller (or this distance).

This makes sense. A condition node to check if there is a new goal on the blackboard seems like a general enough plugin to be useful for other developers. Definitely should be added to the default plugins.

You can use this script to generate a picture of the tree https://github.com/ros-planning/navigation2/blob/master/tools/bt2img.py (also you need to add your new node to the list)

Where do I put the generated image?

[naiveHobo]

What I'm looking for is the ability to have a replan on new goal. Right now with just the rate controller, if the rate is set at 0.001Hz, then if a preemption comes in, it'll still wait 1000s before replanning. Adding a BT condition node for if there's a new goal on the blackboard to replan with would then make it immediate regardless of the rate controller (or this distance).

Having looked at this a bit more, it looks like this is not the job for a BT condition node. I'll continue this conversation #1701.
It seems like this behavior should be implemented within the decorator plugin and enabled or disabled based on a flag set as an input port in the BT. Implementing this behavior is easy enough, I actually already have a condition node for this, but I can't figure out how to place it in the tree such that it can be used along with a decorator node.

[naiveHobo]

Ref BehaviorTree/BehaviorTree.CPP#106 (comment)
Can you look at the BT that Davide proposed in this comment. It would be great to reformat the BT to use this node in that way. What I'm looking for is the ability to have a replan on new goal.

Looking at the BT proposed in the discussion and also with problem I'm facing with integrating the IsNewGoal condition node with the decorator nodes, I think adding condition nodes for time and distance seems like the way to go. All three condition nodes are used in a fallback to trigger a replanning event.

[SteveMacenski]

Where do I put the generated image?

For now, in this PR. Also https://github.com/ros-planning/navigation2/tree/master/nav2_bt_navigator in this readme, you'll see there's sections for each of the BTs in the behavior trees directory. Add your own section / image for this new BT.

Having looked at this a bit more, it looks like this is not the job for a BT condition node. I'll continue this conversation #1701.
I think adding condition nodes for time and distance seems like the way to go

How are you thinking about rectifying the AND or OR for the conditions?

Sorry, I'm also getting a little lost, there's 3 different PRs / work around the behavior tree changes including the ability to preempt that @gimait is working on (#1644) in adding a new goal available plugin for making the planner replan quickly.

This PR is on the distance controller, lets try to keep them separate, sorry about that. What are you working on with the IsNewGoal? That I think is what @gimait is doing so I'm a little confused in the overlap. Lets continue the discussion about how to integrate the distance and time together in #1701 and we can remove the erraneous BT this PR creates if we like that solution.

Trying to keep thing a little structured. It seems like all 3 of these things are starting to blend together and I want to nip that in the butt early so that I'm not infinitely confused.

[SteveMacenski]

Just add the plugin to this list and LGTM https://github.com/ros-planning/navigation2/blob/master/nav2_bt_navigator/src/bt_navigator.cpp#L35

And migration guide from this comment: ros-navigation/docs.nav2.org#13

Just to verify, you actually ran a robot in a simulation or something and made sure this BT worked with initial conditions / preemption / multiple requests / cancellation, correct? Just to make sure the external BT edge cases handle your state tracking correctly.

@crdelsey can you review?

[naiveHobo]

Yes, tested everything in the simulation.

[codecov]

Codecov Report

Merging #1699 into master will increase coverage by 0.75%.
The diff coverage is 76.74%.

@@            Coverage Diff             @@
##           master    #1699      +/-   ##
==========================================
+ Coverage   63.41%   64.16%   +0.75%     
==========================================
  Files         192      194       +2     
  Lines       10321    10364      +43     
==========================================
+ Hits         6545     6650     +105     
+ Misses       3776     3714      -62     

Flag	Coverage Δ
#project	64.16% <76.74%> (+0.75%)	⬆️

Impacted Files	Coverage Δ
nav2_bt_navigator/src/bt_navigator.cpp	78.86% <ø> (ø)
...ior_tree/plugins/decorator/distance_controller.cpp	71.42% <71.42%> (ø)
...ior_tree/plugins/decorator/distance_controller.hpp	100.00% <100.00%> (ø)
nav2_util/include/nav2_util/geometry_utils.hpp	100.00% <100.00%> (ø)
nav2_amcl/src/amcl_node.cpp	83.38% <0.00%> (-0.61%)	⬇️
nav2_controller/src/nav2_controller.cpp	78.64% <0.00%> (+0.52%)	⬆️
nav2_navfn_planner/src/navfn_planner.cpp	82.65% <0.00%> (+2.31%)	⬆️
...v2_recoveries/include/nav2_recoveries/recovery.hpp	86.76% <0.00%> (+8.82%)	⬆️
... and 1 more

---

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 13633cd...cb772f6. Read the comment docs.

[SteveMacenski]

I would like @crdelsey to take a look at this before merging for a second look

[SteveMacenski]

@naiveHobo can you append test_ to the file names dummy_tree_node.hpp transform_handler.cpp transform_handler.hpp? It helps with cover coverage ignoring test files. Any reason the transform_handler can't be a header only file?

[SteveMacenski]

Got some linter updates

Make sure to run ament_cpplint and ament_uncrustify locally

[gimait]

Would it be interesting to use condition nodes like these nested? Like have this, plus the rate controller, and some others. Would that work? It might be a good alternative to the condition nodes.

[naiveHobo]

I did try the type of BT you're suggesting. What happens is that such a decorator directly throttles its child, which means that in a nested structure, the RateController will tick the DistanceController at a given rate, which means the DistanceController will check for the distance traveled at the desired frequency but finally it will be the DistanceController that decided whether to replan or not.

[gimait]

I did try the type of BT you're suggesting. What happens is that such a decorator directly throttles its child, which means that in a nested structure, the RateController will tick the DistanceController at a given rate, which means the DistanceController will check for the distance traveled at the desired frequency but finally it will be the DistanceController that decided whether to replan or not.

There is also the possibility of placing the distance controller on top of the rate controller (it's a matter of defining priorities, I think). I tried something like this with a goalUpdated behaviour and I saw a different (fixable) problem with the executeTick() function (it sets the node status to that returned by the tick function). We can go over this by overriding the executeTick function maybe.
Anyways, that's a matter for another ticket I think.

[SteveMacenski]

@naiveHobo please rebase on new master and retrigger CI, a bunch changed

@gimait should any of that block this PR?

[naiveHobo]

@gimait can you review? This should be okay to merge now.

[SteveMacenski]

Yeah - waiting on a second look from someone else.

An aside; from the pending PRs, we're adding a bunch of BTs. Maybe we should do a review of them and remove the ones that are clearly deprecated, combine ones that make sense, and come up with some sensible default.

[naiveHobo]

Everything seems to be working, tested on the simulation. I haven't added the DistanceController to the default BTs since RateController looks like a better option as the default (till the time SpeedController is integrated). The READMEs and navigation.ros.org have also been updated already.

Maybe we should do a review of them and remove the ones that are clearly deprecated, combine ones that make sense, and come up with some sensible default.

Let's get this one merged first mainly for the test framework. Then #1705 can update and clean up the bunch of BTs and produce a good default.

[SteveMacenski]

Sounds good to me. I agree, this should not be default for the issues we're talked about in other tickets.