Assistive Teleop

[SteveMacenski]

Create an assisted teleop planner/controller/BT such that someone can teleop a robot and make it do things but use costmap information to make sure it doesn't run into anything or go somewhere it shouldn't. Maybe have an elevated permission BT branch so that someone can send commands to collide if appropriate.

Potentially make it semi-autonomous as well

Node take in output of joystick, teleop pad, etc and test that command til N seconds in the future for validity. That way always N seconds from collision at the given speed and vector commanded. Do so in the BT or as a planning plugin?

Or how nav1 did it https://github.com/ros-planning/navigation_experimental/blob/melodic-devel/assisted_teleop

Do a bit of research, find the options, select the most sensible for the behavior tree and nav2 structure.

Might be useful to have a teleop global recovery for a user to joystick a robot out of a bad situation and hit "ok" to go back to autonomous navigation via the BT.

[SteveMacenski]

3 major modalities to support

• standalone teleop node
• Bt node / recovery
• Main task of the robot to do telepresence

As such, should probably contain a core library that is then used to create an independent node, a server for BT recovery, and a BehaviorTree XML specifically configured to do solely assisted teleop.

This should be based on the local costmap and local costmap footprint definitions (possibly a different footprint possible?)

[SteveMacenski]

Merging imminent, waiting on resolution to #3014 before merging but it is approved and ready to rock and roll (just don't want to release a safety feature when it is known to not work due to another unrelated bug)