standalone assisted teleop (ros-navigation#2904)

* standalone assisted teleop

* added in action message

* code review

* moved to behavior server

* added assisted teleop bt node

* revert

* added bt node for assisted teleop

* lint fix

* added cancel assisted teleop node

* code review

* working

* cleanup

* updated feeback

* code review

* update compute velocity

* cleanup

* lint fixes

* cleanup

* test fix

* starting to add tests for assisted teleop

* fixed tests

* undo

* fixed test

* is_recovery

* adjust abort result based on recovery or not

* code review

* added preempt velocity

* working preempt assisted teleop test

* completed assisted teleop tests

* code review

* undo

* code review

* remove sleep

* topic rename

* missing comma

* added comma :(

* added comma

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

nav2_bringup/params/nav2_multirobot_params_1.yaml

@@ -56,6 +56,7 @@ bt_navigator:
  - nav2_follow_path_action_bt_node
  - nav2_spin_action_bt_node
  - nav2_wait_action_bt_node
+ - nav2_assisted_teleop_action_bt_node
  - nav2_back_up_action_bt_node
  - nav2_drive_on_heading_bt_node
  - nav2_clear_costmap_service_bt_node
@@ -93,6 +94,7 @@ bt_navigator:
  - nav2_wait_cancel_bt_node
  - nav2_spin_cancel_bt_node
  - nav2_back_up_cancel_bt_node
+ - nav2_assisted_teleop_cancel_bt_node
  - nav2_drive_on_heading_cancel_bt_node
 
 controller_server:

nav2_bringup/params/nav2_multirobot_params_2.yaml

@@ -56,6 +56,7 @@ bt_navigator:
  - nav2_follow_path_action_bt_node
  - nav2_spin_action_bt_node
  - nav2_wait_action_bt_node
+ - nav2_assisted_teleop_action_bt_node
  - nav2_back_up_action_bt_node
  - nav2_drive_on_heading_bt_node
  - nav2_clear_costmap_service_bt_node
@@ -93,6 +94,7 @@ bt_navigator:
  - nav2_wait_cancel_bt_node
  - nav2_spin_cancel_bt_node
  - nav2_back_up_cancel_bt_node
+ - nav2_assisted_teleop_cancel_bt_node
  - nav2_drive_on_heading_cancel_bt_node
 
 controller_server:

nav2_bringup/params/nav2_params.yaml

@@ -56,6 +56,7 @@ bt_navigator:
  - nav2_follow_path_action_bt_node
  - nav2_spin_action_bt_node
  - nav2_wait_action_bt_node
+ - nav2_assisted_teleop_action_bt_node
  - nav2_back_up_action_bt_node
  - nav2_drive_on_heading_bt_node
  - nav2_clear_costmap_service_bt_node
@@ -93,6 +94,7 @@ bt_navigator:
  - nav2_wait_cancel_bt_node
  - nav2_spin_cancel_bt_node
  - nav2_back_up_cancel_bt_node
+ - nav2_assisted_teleop_cancel_bt_node
  - nav2_drive_on_heading_cancel_bt_node
 
 controller_server:
@@ -278,7 +280,7 @@ behavior_server:
  costmap_topic: local_costmap/costmap_raw
  footprint_topic: local_costmap/published_footprint
  cycle_frequency: 10.0
- behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
+ behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
  spin:
  plugin: "nav2_behaviors/Spin"
  backup:
@@ -287,6 +289,8 @@ behavior_server:
  plugin: "nav2_behaviors/DriveOnHeading"
  wait:
  plugin: "nav2_behaviors/Wait"
+ assisted_teleop:
+ plugin: "nav2_behaviors/AssistedTeleop"
  global_frame: odom
  robot_base_frame: base_link
  transform_tolerance: 0.1