Drive on heading #2898

jwallace42 · 2022-04-07T20:20:30Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#2888
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo

Description of contribution in a few bullet points

Extended the backup behavior to drive on heading. The behavior can now go forwards or backwards.

Description of documentation updates required from your changes

Changes to the behavior server docs and bt nodes docs.

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

mergify · 2022-04-07T20:21:04Z

@jwallace42, please properly fill in PR template in the future. @SteveMacenski, use this instead.

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

SteveMacenski · 2022-04-07T23:07:24Z

What you suggest is definitely not unreasonable, but it is a very large change which will effect every user's configurations since that plugin will cease to exist and all behavior trees will need a different BT node used. I would like to be cautious due to that to try to make this as least disruptive as possible.

Semantically, I think about "back up" versus "drive straight" as 2 different action contexts even though they're representing related things.

Within the BT, would it be possible to have a derived class from drive on heading for backup so that we still have a backup BT node that can be used in the BTs for semantically different tasks? So then we'd have both BT nodes available requiring no changes to end users BTs but then have the additional drive at current heading action (back up just checks its negative). Same with cancel.

I'm debating what I would want for the plugins in the behavior server. From one perceptive, we could do the analog thing here to have both a drive on current heading + backup behavior plugins. But that feels really redundant, and the BT node of backup could just call the renamed behavior plugin for the same results -- we just need to enforce the negative value in the backup version of the BT node. I don't think we need to duplicate the plugins code. What do you think?

I've never done this before, but I think if you created 2 instances in here https://github.com/ros-planning/navigation2/blob/a32ebfaa0ca161de9f6a0841fc1a536c3a0e75ba/nav2_behaviors/behavior_plugin.xml#L9 of the name "backup" and of "driveonheading", then it should make it so that you can load this same plugin based on both names if specified in a parameter file. You'd have to check though. If we did that, we could not duplicate any code, but also not require users to update their YAML files based on the change to load. We could add a depreciation warning into the description.

That leaves the only open question is the action topic ID. For the high level BT node, both can use the drive on current heading name. Or, for the backup version, we could use the original backup name when it is registered.

For the behavior plugin, the drive on current heading is fine. If we aliased the backup plugin loading name, I think it would load the plugin correctly, but if we didn't replace the behavior ID in the yaml from backup to drive_on_heading, then it would use the backup action topic which is wrong, I believe. So I think that implies then that the backup BT node should also use the backup moniker to prevent the need of users to change anything for the current behavior.

but that would brake the use of backup BT node to hook up to the drive at current heading server, without manually overriding it in the BT XML server_name field. So its backward compatibility or the ability moving forward to use semantic use of both back up and drive at heading. Or just duplicating both the BT XML and the behavior plugin to have both.

Options, options, options... I need to think about this. I'm leaning towards duplication though so we can have both forward and backward compatibility, even though they're pretty much the same thing. But if we use derived classes, we can at least reduce the code duplication to the bare minimum on registering topic IDs and checking negative signs.

At some point in the medium term or distant future, we can deprecate backup action, but I fear doing it right now given the already big change between recovery and behavior server already made in this distribution upgrade from galactic -> humble. Another side of the coin is "yolo" because we already have changed so much, may as well just load into it and make 1 very annoying transition vs several minorly irritating transitions.

Thoughts?

jwallace42 · 2022-04-08T14:28:41Z

It is a lot of changes but the only thing they have to do is switch the BT xml file right? Just change the node names and switch sign of the values.

But I really don't know much about the how the upgrading from galactic to humble works. It there something else they would have to do?

mergify · 2022-04-08T16:08:34Z

This pull request is in conflict. Could you fix it @jwallace42?

SteveMacenski · 2022-04-11T18:10:09Z

It is a lot of changes but the only thing they have to do is switch the BT xml file right? Just change the node names and switch sign of the values.

If you derive from this as a base class and make a derived class for backup, it would be the sign and when registering the plugin, using the backup name / action rather than drive on heading.

But I really don't know much about the how the upgrading from galactic to humble works. It there something else they would have to do?

If we didn't make it easy for them, they'd have to modify their configs and BT XMLs because the backup action wouldn't exist and the backup BT node wouldn't exist anymore.

jwallace42 · 2022-04-11T19:58:06Z

Alright, so the goal is still support old BT xml. I will just create a derived backup class. Would there be a plan to faze out the backup node or would we always have it?

SteveMacenski · 2022-04-12T17:19:09Z

This is a good question. I think the BT node would be there forever, I think the actual backend separate action server / behavior plugin would be deprecated. The Backup BT node would just be redirected to that new server after the next distribution release.

I think "DriveOnHeading" vs "BackUp" are semantically different to a autonomy system designer, so having both would make more clear in design context the intent of the move, so both would be nice.

I figure once we have the drive on heading + rotate, we can do a little demo of having the robot drive in a shape or something (e.g. drive 1 meter, turn 90 deg, drive 1 meter, turn 90 deg, ...) which could be helpful for calibration and a POC of its use. Those primitives are common for students in entry-level robotics courses to need to use, so I figure having them around is helpful. Updating the Python API with them would be even more useful. But that takes like 15 minutes, not a big deal

nav2_behavior_tree/CMakeLists.txt

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

nav2_system_tests/src/costmap_filters/speed_local_params.yaml

SteveMacenski

Really good -- if I wasn't trying to reduce code duplication, I think we could even call it done right here.

Something I think worth trying, it might not work out due to complexities, but I think it should at least from a architect's 30,000 ft view:

I looked at what is involved at deriving backup from the drive on heading BT node, and it literally turns into just as much code, just shuffled around. So no need to do that.
I looked at what's involved at deriving backup behavior from the drive on heading behavior plugins, and I think you can reduce essentially all the code. Then you just need to export the plugin and set the different action definition. I think that might need to make DriveOnHeading to be itself templated with a default to DriveOnHeading's message. But that should be a few just method definition changes, in exchange, we have no duplicate code in the behavior plugins for duplicate behavior.

nav2_bringup/launch/tb3_simulation_launch.py

nav2_core/README.md

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp

nav2_behaviors/plugins/drive_on_heading.cpp

jwallace42 · 2022-04-18T14:26:47Z

Fixed of test errors. The xml linter complains but the the structure is actually correct.

SteveMacenski · 2022-04-18T16:57:56Z

In XML < or & are strictly illegal to use as values. There are more which are not strictly illegal but are better avoided. Use an entity reference instead. So your XML file generates an error. The entity reference for < is < and > is >

I think that should help? from https://stackoverflow.com/questions/22463154/unescaped-not-allowed-in-attributes-values-error-in-r

If my memory doesn't fault me, I think I've seen that in use other places in ROS as well for this reason

nav2_behaviors/behavior_plugin.xml

nav2_behaviors/plugins/drive_on_heading.hpp

nav2_behaviors/plugins/drive_on_heading.cpp

jwallace42 · 2022-04-20T19:53:19Z

@SteveMacenski should be gtg!

SteveMacenski

Just some changes on parameterization, but otherwise LGTM!

Did you test manually adding the drive on heading BT XML node in a BT and it worked as you expect?

nav2_bringup/launch/bringup_launch.py

nav2_bringup/launch/tb3_simulation_launch.py

jwallace42 · 2022-04-28T13:04:02Z

Just some changes on parameterization, but otherwise LGTM!

Did you test manually adding the drive on heading BT XML node in a BT and it worked as you expect?

I tested it though the behavior tree by manually adding it and it worked as expected. I also made a bunch of action calls to drive_on_heading.

nav2_behaviors/plugins/drive_on_heading.hpp

SteveMacenski · 2022-04-28T17:38:36Z

@jwallace42 LGTM last is docs:

Add to navigation plugins list in docs
Add parameters page for this

Also test coverage is missing https://codecov.io/gh/ros-planning/navigation2/tree/c04e9f73a7f61022ee8bbb7f5a562c86f5354887/nav2_behaviors/plugins

No testing for backup behavior
No testing for drive on heading behavior

Well actually the testing bits might be due to caching issues. In the circleCI directory, change these 3 instances of v9 to v11 https://github.com/ros-planning/navigation2/blob/main/.circleci/config.yml#L36-L61 to break the cache. But make sure to test and look at the codecov results

mergify · 2022-04-28T17:42:11Z

@jwallace42, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

nav2_behaviors/plugins/drive_on_heading.hpp

SteveMacenski · 2022-04-28T22:04:50Z

https://codecov.io/gh/ros-planning/navigation2/tree/8f722714e31601911b7bcd8515225a4ac13cb7bf/nav2_behaviors/plugins

A bit more test coverage on these would be good -- the cache breaking fixed the issue. This PR would drop the coverage % below 90% for the package

* renamed class * rename backup to drive_on_heading * test file rename * renamed backup bt node * renamed ports * renamed binaries * named backup cancel * renamed backup action * rename yaml * fixing tests * undid cmake * add backup back in for old users * added back_up back in * remove commented tests * rename server * code review * code review fixes * code review * templated the drive on heading * fully templated drive on heading * template fixes for drive on heading * template working * linting and boolean fix * fixed tests * added multiple files for recoveries * xml fix * fixed copyright * revert * Update nav2_behaviors/plugins/drive_on_heading.hpp * Update nav2_behaviors/plugins/drive_on_heading.hpp * update circleci config * fixed back_up test * fixed drive on heading test Co-authored-by: Steve Macenski <[email protected]>

jwallace42 added 9 commits April 6, 2022 11:19

renamed class

0f25db7

rename backup to drive_on_heading

f3d1df8

test file rename

1857b22

renamed backup bt node

ccc952a

renamed ports

8f4aaff

renamed binaries

21e6665

named backup cancel

34b4ecc

renamed backup action

3bf6f42

rename yaml

a32ebfa

fixing tests

e605a99

undid cmake

0d41190

add backup back in for old users

067f9c2

SteveMacenski reviewed Apr 12, 2022

View reviewed changes

jwallace42 added 7 commits April 13, 2022 09:55

Merge branch 'main' into drive_on_heading

e64a23c

added back_up back in

2d7c418

remove commented tests

d43df2c

rename server

9ddcd63

code review

bcdcf05

code review fixes

8566a76

Merge branch 'main' into drive_on_heading

e2c0ad0

jwallace42 requested a review from SteveMacenski April 13, 2022 16:38

SteveMacenski reviewed Apr 13, 2022

View reviewed changes

code review

5bd6234

jwallace42 added 2 commits April 17, 2022 21:05

linting and boolean fix

6604c7b

fixed tests

b116e91

jwallace42 requested a review from SteveMacenski April 18, 2022 16:54

SteveMacenski reviewed Apr 18, 2022

View reviewed changes

jwallace42 added 2 commits April 19, 2022 15:23

added multiple files for recoveries

bb09abb

xml fix

aa58be2

jwallace42 requested a review from SteveMacenski April 19, 2022 21:29

fixed copyright

2dcca64

SteveMacenski requested changes Apr 27, 2022

View reviewed changes

nav2_bringup/launch/bringup_launch.py Outdated Show resolved Hide resolved

nav2_bringup/launch/tb3_simulation_launch.py Outdated Show resolved Hide resolved

revert

c04e9f7

jwallace42 requested a review from SteveMacenski April 28, 2022 15:08

SteveMacenski approved these changes Apr 28, 2022

View reviewed changes

nav2_behaviors/plugins/drive_on_heading.hpp Outdated Show resolved Hide resolved

Update nav2_behaviors/plugins/drive_on_heading.hpp

08d721a

SteveMacenski reviewed Apr 28, 2022

View reviewed changes

nav2_behaviors/plugins/drive_on_heading.hpp Outdated Show resolved Hide resolved

SteveMacenski and others added 3 commits April 28, 2022 10:51

Update nav2_behaviors/plugins/drive_on_heading.hpp

8195300

Merge branch 'main' into drive_on_heading

096727b

update circleci config

8f72271

jwallace42 added 2 commits May 3, 2022 11:13

fixed back_up test

fa06797

fixed drive on heading test

41ff070

SteveMacenski merged commit 54323dc into ros-navigation:main May 4, 2022

jwallace42 deleted the drive_on_heading branch May 9, 2022 18:50

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Drive on heading #2898

Drive on heading #2898

jwallace42 commented Apr 7, 2022

mergify bot commented Apr 7, 2022

SteveMacenski commented Apr 7, 2022 •

edited

Loading

jwallace42 commented Apr 8, 2022

mergify bot commented Apr 8, 2022

SteveMacenski commented Apr 11, 2022

jwallace42 commented Apr 11, 2022

SteveMacenski commented Apr 12, 2022

SteveMacenski left a comment

jwallace42 commented Apr 18, 2022

SteveMacenski commented Apr 18, 2022 •

edited

Loading

jwallace42 commented Apr 20, 2022

SteveMacenski left a comment

jwallace42 commented Apr 28, 2022 •

edited

Loading

SteveMacenski commented Apr 28, 2022 •

edited

Loading

mergify bot commented Apr 28, 2022

SteveMacenski commented Apr 28, 2022

Drive on heading #2898

Drive on heading #2898

Conversation

jwallace42 commented Apr 7, 2022

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

mergify bot commented Apr 7, 2022

SteveMacenski commented Apr 7, 2022 • edited Loading

jwallace42 commented Apr 8, 2022

mergify bot commented Apr 8, 2022

SteveMacenski commented Apr 11, 2022

jwallace42 commented Apr 11, 2022

SteveMacenski commented Apr 12, 2022

SteveMacenski left a comment

Choose a reason for hiding this comment

jwallace42 commented Apr 18, 2022

SteveMacenski commented Apr 18, 2022 • edited Loading

jwallace42 commented Apr 20, 2022

SteveMacenski left a comment

Choose a reason for hiding this comment

jwallace42 commented Apr 28, 2022 • edited Loading

SteveMacenski commented Apr 28, 2022 • edited Loading

mergify bot commented Apr 28, 2022

SteveMacenski commented Apr 28, 2022

SteveMacenski commented Apr 7, 2022 •

edited

Loading

SteveMacenski commented Apr 18, 2022 •

edited

Loading

jwallace42 commented Apr 28, 2022 •

edited

Loading

SteveMacenski commented Apr 28, 2022 •

edited

Loading