Add BT based implementation of waypoint_follower

[ymd-stella]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	related to #1651 (another implementation of #803)
Primary OS tested on	Ubuntu
Robotic platform tested on	None

---

Description of contribution in a few bullet points

• Add BT based implementation of waypoint_follower (Action server of FollowWaypoints)
  • Customizable recovery behavior will be available for FollowWaypoints (As in the case of NavigateToPose)
  • This is also an example of NavigateToPose BT-node

Description of documentation updates required from your changes

• Update README.md (Build Status)
• Update navigation.ros.org

---

Future work that may be required in bullet points

• Decide how to deal with the existing waypoint_follower

the goals of this contribution and the scope of it

• Provide customizable version of waypoint_follower (Action server of FollowWaypoints) named nav2_bt_waypoint_follower, and replace nav2_waypoint_follower with nav2_bt_waypoint_follower.
• Provide an example of NavigateToPose BT-node

[codecov]

Codecov Report

Merging #1928 into main will decrease coverage by 2.10%.
The diff coverage is 92.77%.

@@            Coverage Diff             @@
##             main    #1928      +/-   ##
==========================================
- Coverage   78.18%   76.08%   -2.11%     
==========================================
  Files         219      228       +9     
  Lines       10588    11029     +441     
==========================================
+ Hits         8278     8391     +113     
- Misses       2310     2638     +328     

Flag	Coverage Δ
#project	?

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files	Coverage Δ
...t_follower/plugins/action/get_next_goal_action.cpp	85.71% <85.71%> (ø)
...plugins/condition/all_goals_achieved_condition.cpp	92.85% <92.85%> (ø)
..._bt_waypoint_follower/src/bt_waypoint_follower.cpp	93.54% <93.54%> (ø)
...t_follower/plugins/action/get_next_goal_action.hpp	100.00% <100.00%> (ø)
...plugins/condition/all_goals_achieved_condition.hpp	100.00% <100.00%> (ø)
nav2_bt_waypoint_follower/src/main.cpp	100.00% <100.00%> (ø)
nav2_navfn_planner/src/navfn.cpp	85.58% <0.00%> (-5.28%)	⬇️
...tmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp	69.23% <0.00%> (-4.11%)	⬇️
nav2_costmap_2d/src/clear_costmap_service.cpp	24.21% <0.00%> (-1.57%)	⬇️
nav2_costmap_2d/src/costmap_2d_publisher.cpp	45.16% <0.00%> (-1.30%)	⬇️
... and 60 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 0a2dfbe...7cb909c. Read the comment docs.

[SteveMacenski]

I wasn't able to fully analyze this, but please make sure that you change so that you're adding a new package doing some behavior tree autonomy and not replacing the existing one.

Also in your PR, a little section about the goals of this contribution and the scope of it (autonomy applications only, are you seeing this as some replacement for bt navigator? etc). There seems to be some feature creep between them we should flush out and make clear the separation of concerns that this has w.r.t. BT navigator and the navigation task

[ymd-stella]

please make sure that you change so that you're adding a new package doing some behavior tree autonomy and not replacing the existing one.

Why?
(Did it look to you like I edited nav2_waypoint_follower?)

[ymd-stella]

Also in your PR, a little section about the goals of this contribution and the scope of it

I've added it.

[SteveMacenski]

(Did it look to you like I edited nav2_waypoint_follower?)

It looks like you deleted the waypoint follower package and replaced it with the bt waypoint follower (or just modified the waypoint follower and overrided it as a new package)

[ymd-stella]

What am I missing in this PR?

[SteveMacenski]

@mkhansenbot what's your take on this?

Its another waypoint follower using a behavior tree as the high level application (using the NavigateToPose BT to call the BT navigator). In concept I don't have an issue with it, but there's a bunch of redundant code between them that makes me think we should have some more wrappers in the nav2_behavior_tree package to deal with the boilerplate.

Additionally, it's really not a 'waypoint follower' as much as the default behavior trees given are for following tasks. It really can be thought more of the "autonomy application framework" where you can use any behavior tree nodes to create your autonomy application as you like. That to me makes me think a different name / scope is appropriate and we should provide a couple of BT options to make the point clear. E.g.

• Waypoint following and [do something at waypoint] example BT (eg pick up object, take a picture, get a box, etc)
• Waypoint following example BT
• Something else, like a patrolling route BT or something non-predefined-waypoint

If for some reason we couldn't generalize this for an autonomy application, then maybe this is better suited for the navigation2_tutorials repo and write a tutorial about making a BT autonomy application per #1651 with some example application behind just waypoint following.

@ymd-stella pull in main, your fork is too old to run CI

[mkhansenbot]

I like the idea of adding a BT which adds a more real use case than just navigate to a series of waypoints. The "take a picture" could be used for a patrolling use case, the "pick up object" could be a future case for robots with arms. The whole purpose of moving to a BT based approach was to make these extensions possible by adding additional BT actions. These were on our list of desired examples since we started working on Nav2. As for whether those belong in this repo or not, that's a matter of opinion. My opinion is that these types of example apps should be moved into another repo, including the existing C++ waypoint follower example.

[SteveMacenski]

I'd tend to agree, but I've gotten feedback along the lines of "thank god we have the waypoint follower in the navigation stack now" as if just having it somewhere else would somehow make it insufficient. Within reason, I think another "bt application" package could be added as long as it was generalizable with a few BTs to make the point. If we can't generalize it, then I think it shoiuld go in the nav2 tutorials repo or we make a new repo for nav2 applications.

@ymd-stella is there another action message we can define to make this BT application more general and just now the FollowWaypoints action? Any naming ideas? The aim is to be the generic package for building your business specific applications (go to waypoint and get box for warehouse; follow waypoints and don't stop for patroling with low battery for return to dock; go to a shelf and pick up an item and drop it off on something else; etc). Do you think we can make a generic action interface to enable that?

[mikeferguson]

Here's a thought, based on managing dependencies:

• Create a different repo that holds the core infrastructure for this larger "BT application framework" (infrastructure here mainly means the plugin definitions and ROS2 interfaces that every plugin will have to depend on).
• Then setup the new waypoint follower to use this core infrastructure, it and other nav-related portions could remain in this repo.
• Then things like "take a picture" and other demos, could be located either with the framework repo, or to better manage dependencies put in an examples repo.

The big reason to do this is managing dependencies. If you keep the framework inside nav2, then things like MoveIt, etc would have to depend on nav2 directly if they wanted to keep their "BT application" nodes inside their repo. Having a lightweight framework repo will almost certainly aid in adoption of the new framework.

[ymd-stella]

is there another action message we can define to make this BT application more general and just now the FollowWaypoints action?

I can think of the following action like NavigateToPose to pass the BT filename. I'll call it the PerformTask action temporarily.

#goal definition
string behavior_tree
---
std_msgs/Empty result
---
geometry_msgs/PoseStamped current_pose

If I want the robot to do go to waypoint and get box for warehouse, I create the following file and pass it to the PerformTask action.

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="PerformTask">
      <NavigateToPose goal="0;map;5.0;0.0;0.0;0.0;0.0;0.0;1.0"/> <!-- go to point A -->
      <GetBox/>
      <NavigateToPose goal="0;map;0.0;0.0;0.0;0.0;0.0;0.0;1.0"/> <!-- go to point B -->
      <PutBox/>
    </Sequence> <!-- Move a box from A to B -->
  </BehaviorTree>
</root>

The disadvantage of this configuration is that we lose the mechanism to give an array of goals.
Therefore, I think it's difficult to generalize this while keeping it functional.

I think it's a good idea to move it to a different repository.
(It's debatable whether it's better to use navigation2_tutorials or to create a new repository for nav2 application (or demo).)

[SteveMacenski]

I'm wondering about templating the this server instead so that they can implement it with whatever interface they like, under some light weight requirements (like certain fields must be in the result like a return code). That way you can make your own action interface for whatever your aim is but still use this server as the basis. I'm thinking of the behavior tree action nodes in nav2_behavior_tree which all derive from a templated base class as an example. Then we would have some on_feedback_update() methods for the user to implement to handle the specifics of their application. Though that implies they'd need to derive from this, it wouldn't just be a 'here's a message, do it'.

Another option is having the request be a little more generic. For example, in your provided BT, we if were able to change it to something like

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="PerformTask">
      <NavigateToPose goal=${goalA}/> <!-- go to point A -->
      <GetBox/>
      <NavigateToPose goal=${goalB}/> <!-- go to point B -->
      <PutBox/>
    </Sequence> <!-- Move a box from A to B -->
  </BehaviorTree>
</root>

So the action request would have a behavior tree field, but then also some array of info corresponding to these input port / blackboard variables for specific ports like goal max_speed timeout or whatever ports are necessary.

Then the goal request field would be something like:

string behavior_tree
Something[] ports

And Something would be a map or something of the ports. I think there may be a clever way to format this to do the common things like static variables timeout: 10.4 max_speed: 0.453 but also longer arrays like goals: {goalA, goalB, ... goalN} for a waypoint following task we load onto the blackboard for use as goals. I know that would work for single-valued things like timeout, but I haven't thought in detail if that would be possible for arrays of goals. If we can make an array of goals, goals, then I think this could potentially work. For the box example, we'd have 2 of those arrays; 1 for drop off, 1 for pick up: goalsA: {goal1, goal2, ... goalN}, goalsB: {goal1, goal2, ... goalN}. For just go-to-waypoint, there'd just be 1 list of them. For dynamic tasks where the goal updates over a topic (like the dynamic point following demo) then we wouldn't have to specify these at run-time and the updated goals would come over a topic the BT would have a node listening to to update.

It wouldn't be hard to have a BT node look at the blackboard list of waypoints and dish them out on the goal port too, so that the complexity of using the ports would be on the BT. The major issue there is being able to have a message take in that dynamic of a range of things in some Something message to even start.

Thoughts?

[ymd-stella]

I'll try to implement it.
we'll add a module for mapping from BT to Action (like BtActionNode mapping from Action to BT).
I think I'll request a review about it in another PR.

[SteveMacenski]

I think you should describe what you're going to do / aims before going off and doing it, just to make sure we're aligned so that you don't accidentally spend a bunch of time on something that might not quite fit

[SteveMacenski]

Waiting on some progress in #2010 since that would likely be the backbone of this work

[SteveMacenski]

@ymd-stella I'm going to close this PR since the new refactored version probably makes sense for this PR to have a clean history once (if) you get a chance to do it with the new abstracted logic. It should be much simpler now with alot less code redundancy!