From f8471d42eaa40ef1952ee0243d40fe576e07b919 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 30 May 2022 20:21:00 -0400 Subject: [PATCH 1/5] added docs for assisted teleop behavior and bt node --- .../bt-plugins/actions/AssistedTeleop.rst | 65 +++++++++++++++++++ .../packages/configuring-behavior-server.rst | 20 +++++- configuration/packages/configuring-bt-xml.rst | 1 + 3 files changed, 85 insertions(+), 1 deletion(-) create mode 100644 configuration/packages/bt-plugins/actions/AssistedTeleop.rst diff --git a/configuration/packages/bt-plugins/actions/AssistedTeleop.rst b/configuration/packages/bt-plugins/actions/AssistedTeleop.rst new file mode 100644 index 0000000000..ba22044a4f --- /dev/null +++ b/configuration/packages/bt-plugins/actions/AssistedTeleop.rst @@ -0,0 +1,65 @@ +.. _bt_assisted_teleop_action: + +AssistedTeleop +============== + +Invokes the AssistedTeleop ROS 2 action server, which filters teleop twist commands to prevent +collisions. This is used in nav2 Behavior Trees as a recovery behavior or a regular behavior. +The nav2_behaviors_ module implements the BackUp action server. + +.. _nav2_behaviors: https://github.com/ros-planning/navigation2/tree/main/nav2_behaviors + + +Input Ports +*********** + +:is_recovery: + + ====== ======= + Type Default + ------ ------- + double false + ====== ======= + + Description + If true increment the recovery counter. + +:time_allowance: + + ====== ======= + Type Default + ------ ------- + double 10.0 + ====== ======= + + Description + Time to envoke behavior for, if exceeds considers it a stuck condition or failure case (seconds). + +:server_name: + + ====== ======= + Type Default + ------ ------- + string N/A + ====== ======= + + Description + Action server name. + +:server_timeout: + + ====== ======= + Type Default + ------ ------- + double 10 + ====== ======= + + Description + Action server timeout (ms). + +Example +------- + +.. code-block:: xml + + diff --git a/configuration/packages/configuring-behavior-server.rst b/configuration/packages/configuring-behavior-server.rst index 87ac6960c5..94caec3844 100644 --- a/configuration/packages/configuring-behavior-server.rst +++ b/configuration/packages/configuring-behavior-server.rst @@ -211,6 +211,22 @@ DriveOnHeading distance, speed and time_allowance is given from the action reque Description Time to look ahead for collisions (s). +AssistedTeleop Behavior Parameters +********************************** + +AssistedTeleop time_allowance is given in the action request + +:is_recovery: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool false + ============== ============================= + + Description + If true increment the recovery counter. + Example ******* .. code-block:: yaml @@ -220,7 +236,7 @@ Example 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", "wait", "assisted_teleop"] spin: plugin: "nav2_behaviors/Spin" backup: @@ -229,6 +245,8 @@ Example plugin: "nav2_behaviors/DriveOnHeading" wait: plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link transform_timeout: 0.1 diff --git a/configuration/packages/configuring-bt-xml.rst b/configuration/packages/configuring-bt-xml.rst index b78ba30482..b61c237210 100644 --- a/configuration/packages/configuring-bt-xml.rst +++ b/configuration/packages/configuring-bt-xml.rst @@ -23,6 +23,7 @@ Action Plugins bt-plugins/actions/Spin.rst bt-plugins/actions/BackUp.rst bt-plugins/actions/DriveOnHeading.rst + bt-plugins/actions/AssistedTeleop.rst bt-plugins/actions/ComputePathToPose.rst bt-plugins/actions/FollowPath.rst bt-plugins/actions/NavigateToPose.rst From 6eac4e4c5a3eb247685ad7831b232068ee073adc Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sat, 4 Jun 2022 15:33:09 -0400 Subject: [PATCH 2/5] added more params --- .../packages/configuring-behavior-server.rst | 28 +++++++++++++++++-- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/configuration/packages/configuring-behavior-server.rst b/configuration/packages/configuring-behavior-server.rst index 94caec3844..55267b62c3 100644 --- a/configuration/packages/configuring-behavior-server.rst +++ b/configuration/packages/configuring-behavior-server.rst @@ -216,16 +216,38 @@ AssistedTeleop Behavior Parameters AssistedTeleop time_allowance is given in the action request -:is_recovery: +:projection_time: ============== ============================= Type Default -------------- ----------------------------- - bool false + double 1.0 + ============== ============================= + + Description + Time to look ahead for collisions (s). + +:simulation_time_step: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + Time step for projections (s). + +:teleop_cmd_vel: + + ============== ============================= + Type Default + -------------- ----------------------------- + string teleop_cmd_vel ============== ============================= Description - If true increment the recovery counter. + Topic to listen for teleop messages. Example ******* From c955d5aa46258ddd3e2d1315671c9aeb9dbe348b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 5 Jun 2022 19:22:20 -0400 Subject: [PATCH 3/5] added BT nodes --- .../bt-plugins/actions/AssistedTeleop.rst | 2 +- .../actions/CancelAssistedTeleop.rst | 39 +++++++++++++++++++ configuration/packages/configuring-bt-xml.rst | 1 + plugins/index.rst | 13 ++++++- 4 files changed, 53 insertions(+), 2 deletions(-) create mode 100644 configuration/packages/bt-plugins/actions/CancelAssistedTeleop.rst diff --git a/configuration/packages/bt-plugins/actions/AssistedTeleop.rst b/configuration/packages/bt-plugins/actions/AssistedTeleop.rst index ba22044a4f..5b3e0e0218 100644 --- a/configuration/packages/bt-plugins/actions/AssistedTeleop.rst +++ b/configuration/packages/bt-plugins/actions/AssistedTeleop.rst @@ -5,7 +5,7 @@ AssistedTeleop Invokes the AssistedTeleop ROS 2 action server, which filters teleop twist commands to prevent collisions. This is used in nav2 Behavior Trees as a recovery behavior or a regular behavior. -The nav2_behaviors_ module implements the BackUp action server. +The nav2_behaviors_ module implements the AssistedTeleop action server. .. _nav2_behaviors: https://github.com/ros-planning/navigation2/tree/main/nav2_behaviors diff --git a/configuration/packages/bt-plugins/actions/CancelAssistedTeleop.rst b/configuration/packages/bt-plugins/actions/CancelAssistedTeleop.rst new file mode 100644 index 0000000000..9760eb67f1 --- /dev/null +++ b/configuration/packages/bt-plugins/actions/CancelAssistedTeleop.rst @@ -0,0 +1,39 @@ +.. _bt_cancel_assisted_teleop: + +CancelAssistedTeleop +==================== + +Used to cancel the AssistedTeleop action that is part of the behavior server. The server address can be remapped using the ``server_name`` input port. + +Input Ports +----------- + +:service_name: + + ====== ======= + Type Default + ------ ------- + string N/A + ====== ======= + + Description + Service name, if not using default of ``assisted_teleop`` due to remapping. + + +:server_timeout: + + ====== ======= + Type Default + ------ ------- + double 10 + ====== ======= + + Description + Server timeout (ms). + +Example +------- + +.. code-block:: xml + + diff --git a/configuration/packages/configuring-bt-xml.rst b/configuration/packages/configuring-bt-xml.rst index b61c237210..843aa891e0 100644 --- a/configuration/packages/configuring-bt-xml.rst +++ b/configuration/packages/configuring-bt-xml.rst @@ -44,6 +44,7 @@ Action Plugins bt-plugins/actions/CancelSpin.rst bt-plugins/actions/CancelWait.rst bt-plugins/actions/CancelDriveOnHeading.rst + bt-plugins/actions/CancelAssistedTeleop.rst bt-plugins/actions/Smooth.rst Condition Plugins diff --git a/plugins/index.rst b/plugins/index.rst index daa338060c..25a91e78ce 100644 --- a/plugins/index.rst +++ b/plugins/index.rst @@ -198,12 +198,17 @@ Behaviors | `Drive On Heading`_ | Joshua Wallace | Drive on heading behavior with | | | | configurable distance to drive | +----------------------+------------------------+----------------------------------+ +| `Assisted Teleop`_ | Joshua Wallace | AssistedTeleop behavior that | +| | | scales teleop commands to | +| | | prevent collisions. | ++----------------------+------------------------+----------------------------------+ .. _Back Up: https://github.com/ros-planning/navigation2/tree/main/nav2_behaviors/plugins .. _Spin: https://github.com/ros-planning/navigation2/tree/main/nav2_behaviors/plugins .. _Wait: https://github.com/ros-planning/navigation2/tree/main/nav2_behaviors/plugins .. _Drive On Heading: https://github.com/ros-planning/navigation2/tree/main/nav2_behaviors/plugins .. _Clear Costmap: https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/clear_costmap_service.cpp +.. _Assisted Teleop: https://github.com/ros-planning/navigation2/tree/main/nav2_behaviors/plugins Waypoint Task Executors ======================= @@ -275,6 +280,8 @@ Behavior Tree Nodes +--------------------------------------------+---------------------+------------------------------------------+ | `Drive On Heading Action`_ | Joshua Wallace | Calls drive on heading behavior action | +--------------------------------------------+---------------------+------------------------------------------+ +| `Assisted Teleop Action`_ | Joshua Wallace | Calls assisted teleop behavior action | ++--------------------------------------------+---------------------+------------------------------------------+ | `Clear Entire Costmap Service`_ | Carl Delsey | Calls clear entire costmap service | +--------------------------------------------+---------------------+------------------------------------------+ | `Clear Costmap Except Region Service`_ | Guillaume Doisy | Calls clear costmap except region service| @@ -333,11 +340,14 @@ Behavior Tree Nodes +--------------------------------------------+---------------------+------------------------------------------+ | `Cancel Wait Action`_ |Pradheep Padmanabhan | Cancels wait behavior action | +--------------------------------------------+---------------------+------------------------------------------+ -| `Cancel Drive on Heading Action`_ |Jousha Wallace | Cancels drive on heading behavior action | +| `Cancel Drive on Heading Action`_ | Joshua Wallace | Cancels drive on heading behavior action | ++--------------------------------------------+---------------------+------------------------------------------+ +| `Cancel Assisted Teleop Action`_ | Joshua Wallace | Cancels assisted teleop behavior action | +--------------------------------------------+---------------------+------------------------------------------+ .. _Back Up Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/back_up_action.cpp .. _Drive On Heading Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +.. _Assisted Teleop Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp .. _Clear Entire Costmap Service: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp .. _Clear Costmap Except Region Service: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp .. _Clear Costmap Around Robot Service: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -361,6 +371,7 @@ Behavior Tree Nodes .. _Cancel Spin Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp .. _Cancel Wait Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp .. _Cancel Drive on Heading Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp +.. _Cancel Assisted Teleop Action: https://github.com/ros-planning/navigation2/tree/main/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp +------------------------------------+--------------------+------------------------+ From 142abdf5a8546951bf3dcdbe17d7bac118c89536 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 10 Jun 2022 17:30:02 -0400 Subject: [PATCH 4/5] added assisted teleop to migration guide --- migration/Galactic.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/migration/Galactic.rst b/migration/Galactic.rst index 9fb2938ccb..0ef28df7b9 100644 --- a/migration/Galactic.rst +++ b/migration/Galactic.rst @@ -315,3 +315,7 @@ Goal Checker API Changed ************************ `PR 2965 `_ adds an extra argument in the initialize function of the `nav2_core::GoalChecker` class. The extra argument is a costmap_ros pointer. This is used to check if the goal is in collision, so that we can avoid moving towards the goal and replanning can be initiates using some BT plugin. + +Added Assisted Teleop +********************* +`PR 2965 `_ adds a new behavior for assisted teleop along with two new BT nodes AssistedTeleop and CancelAssistedTeleop. From 1746ced195236775fde6f83ac43558df464f3f72 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 12 Jun 2022 19:29:56 -0400 Subject: [PATCH 5/5] topic rename --- configuration/packages/configuring-behavior-server.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/configuration/packages/configuring-behavior-server.rst b/configuration/packages/configuring-behavior-server.rst index 55267b62c3..6fb5bd4443 100644 --- a/configuration/packages/configuring-behavior-server.rst +++ b/configuration/packages/configuring-behavior-server.rst @@ -238,12 +238,12 @@ AssistedTeleop time_allowance is given in the action request Description Time step for projections (s). -:teleop_cmd_vel: +:cmd_vel_teleop: ============== ============================= Type Default -------------- ----------------------------- - string teleop_cmd_vel + string cmd_vel_teleop ============== ============================= Description