From bcb3fffa0c9bb45d5434b91f8a2bb788463785b0 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 24 Oct 2022 17:37:49 -0400 Subject: [PATCH 1/6] docs for changes to behavior server --- .../packages/configuring-behavior-server.rst | 36 +++++++++++++++---- .../docs/writing_new_behavior_plugin.rst | 8 +++-- 2 files changed, 35 insertions(+), 9 deletions(-) diff --git a/configuration/packages/configuring-behavior-server.rst b/configuration/packages/configuring-behavior-server.rst index 6fb5bd4443..e6beeba3e5 100644 --- a/configuration/packages/configuring-behavior-server.rst +++ b/configuration/packages/configuring-behavior-server.rst @@ -16,7 +16,7 @@ Note: pre-Rolling/Humble this was the Recovery server, not behavior server. Laun Behavior Server Parameters ************************** -:costmap_topic: +:local_costmap_topic: ============== =========================== Type Default @@ -25,9 +25,20 @@ Behavior Server Parameters ============== =========================== Description - Raw costmap topic for collision checking. + Raw costmap topic for collision checking on the local costmap. -:footprint_topic: +:global_costmap_topic: + + ============== =========================== + Type Default + -------------- --------------------------- + string "global_costmap/costmap_raw" + ============== =========================== + + Description + Raw costmap topic for collision checking on the global costmap. + +:local_footprint_topic: ============== =================================== Type Default @@ -36,7 +47,18 @@ Behavior Server Parameters ============== =================================== Description - Topic for footprint in the costmap frame. + Topic for footprint in the local costmap frame. + +:global_footprint_topic: + + ============== =================================== + Type Default + -------------- ----------------------------------- + string "global_costmap/published_footprint" + ============== =================================== + + Description + Topic for footprint in the global costmap frame. :cycle_frequency: @@ -255,8 +277,10 @@ Example recoveries_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + local_costmap_topic: local_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_costmap_topic: global_costmap/costmap_raw + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "wait", "assisted_teleop"] spin: diff --git a/plugin_tutorials/docs/writing_new_behavior_plugin.rst b/plugin_tutorials/docs/writing_new_behavior_plugin.rst index 13002a25e5..e96e453256 100644 --- a/plugin_tutorials/docs/writing_new_behavior_plugin.rst +++ b/plugin_tutorials/docs/writing_new_behavior_plugin.rst @@ -223,13 +223,15 @@ To enable the plugin, we need to modify the ``nav2_params.yaml`` file as below t with -.. code-block:: text +.. code-block:: yaml behavior_server: (Humble and newer) recoveries_server: (Galactic and earlier) ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + local_costmap_topic: local_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_costmap_topic: global_costmap/costmap_raw + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "wait","send_sms"] (Humble and newer) recovery_plugins: ["spin", "backup", "wait","send_sms"] (Galactic and earlier) From 234537a960d396e1a8d981488495642af43419ae Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sat, 10 Dec 2022 18:34:01 -0500 Subject: [PATCH 2/6] doc updates --- .../packages/configuring-behavior-server.rst | 22 ++++++++++--- migration/Humble.rst | 31 +++++-------------- 2 files changed, 24 insertions(+), 29 deletions(-) diff --git a/configuration/packages/configuring-behavior-server.rst b/configuration/packages/configuring-behavior-server.rst index e6beeba3e5..9d37530d3c 100644 --- a/configuration/packages/configuring-behavior-server.rst +++ b/configuration/packages/configuring-behavior-server.rst @@ -82,7 +82,7 @@ Behavior Server Parameters Description TF transform tolerance. -:global_frame: +:local_frame: ============== ============================= Type Default @@ -91,7 +91,18 @@ Behavior Server Parameters ============== ============================= Description - Reference frame. + Local reference frame. + +:global_frame: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "map" + ============== ============================= + + Description + Global reference frame. :robot_base_frame: @@ -122,7 +133,7 @@ Behavior Server Parameters .. code-block:: yaml - recoveries_server: + behavior_server: ros__parameters: behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] spin: @@ -275,7 +286,7 @@ Example ******* .. code-block:: yaml - recoveries_server: + behavior_server: ros__parameters: local_costmap_topic: local_costmap/costmap_raw local_footprint_topic: local_costmap/published_footprint @@ -293,7 +304,8 @@ Example plugin: "nav2_behaviors/Wait" assisted_teleop: plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom + local_frame: odom + global_frame: map robot_base_frame: base_link transform_timeout: 0.1 simulate_ahead_time: 2.0 diff --git a/migration/Humble.rst b/migration/Humble.rst index 563d301b29..b88cd4468c 100644 --- a/migration/Humble.rst +++ b/migration/Humble.rst @@ -66,27 +66,6 @@ The following errors codes are supported (with more to come as necessary): Unkno The following error codes are supported (with more to come as necessary): Unknown, TF Error, Invalid Path, Patience Exceeded, Failed To Make Progress, or No Valid Control. -`PR #3251 `_ pipes the highest priority error code through the bt_navigator and defines the error code structure. - -A new parameter for the the BT Navigator called "error_code_id_names" was added to the nav2_params.yaml to define the error codes to compare. -The lowest error in the "error_code_id_names" is then returned in the action request (navigate to pose, navigate through poses waypoint follower), whereas the code enums increase the higher up in the software stack - giving higher priority to lower-level failures. - -The error codes produced from the servers follow the guidelines stated below. -Error codes from 0 to 9999 are reserved for nav2 while error codes from 10000-65535 are reserved for external servers. -Each server has two "reserved" error codes. 0 is reserved for NONE and the first error code in the sequence is reserved for UNKNOWN. - -The current implemented servers with error codes are: - -- Controller Server: NONE:0, UNKNOWN:100, server error codes: 101-199 -- Planner Server(compute_path_to_pose): NONE:0, UNKNOWN:201, server error codes: 201-299 -- Planner Server(compute_path_through_poses): NONE:0, UNKNOWN:301, server error codes: 301-399 -- Smoother Server: NONE: 0, UNKNOWN: 501, server error codes: 501-599 -- Waypoint Follower Server: NONE: 0, UNKNOWN: 601, server error codes: 601-699 - -This pr also updates the waypoint follower server to throw exceptions on failures. These exceptions get reported back to the server which in turn places a error code on the Behavior Tree Navigator's blackboard for use in contextual error handling in the autonomy application. -The following errors codes are supported (with more to come as necessary): Unknown and Task Executor Failed. -See :ref:`adding_a_nav2_task_server` and the PR for additional information. - Costmap Filters *************** @@ -107,6 +86,10 @@ SmootherSelector BT Node ************************ `PR #3283 `_ adds a BT node to set the smoother based on a topic or a default. See the configuration guide :ref:`SimpleSmoother` for more details. -Publish Costmap Layers -********************** -`PR #3320 `_ adds the ability for the nav2_costmap_2d package to publish out costmap data associated with each layer. \ No newline at end of file +Give Behavior Server Access to Both Costmaps +******************************************** +`PR #3255 `_ addes the ability for a behavior to access both the local and global costmap. + +To update behaviors, any reference to the global_frame must be updated to the local_frame parameter +along with the `configuration` method which now takes in the local and global collision checker. +Lastly, `getResourceInfo` must be overriden to return `CostmapInfoType::LOCAL`. From 5cab436308a2281e971b5ad1a7e4d881141a9d0f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sat, 10 Dec 2022 18:38:42 -0500 Subject: [PATCH 3/6] clean up --- migration/Humble.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/migration/Humble.rst b/migration/Humble.rst index b88cd4468c..7349582aa7 100644 --- a/migration/Humble.rst +++ b/migration/Humble.rst @@ -88,8 +88,8 @@ SmootherSelector BT Node Give Behavior Server Access to Both Costmaps ******************************************** -`PR #3255 `_ addes the ability for a behavior to access both the local and global costmap. +`PR #3255 `_ addes the ability for a behavior to access the local and global costmap. To update behaviors, any reference to the global_frame must be updated to the local_frame parameter -along with the `configuration` method which now takes in the local and global collision checker. -Lastly, `getResourceInfo` must be overriden to return `CostmapInfoType::LOCAL`. +along with the ``configuration`` method which now takes in the local and global collision checkers. +Lastly, ``getResourceInfo`` must be overriden to return ``CostmapInfoType::LOCAL``. From 15aec1e65b864ab37107ab0ead32690e05b6a7d6 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 13 Dec 2022 10:25:22 -0500 Subject: [PATCH 4/6] revert --- migration/Humble.rst | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/migration/Humble.rst b/migration/Humble.rst index 7349582aa7..40764aa98b 100644 --- a/migration/Humble.rst +++ b/migration/Humble.rst @@ -66,6 +66,27 @@ The following errors codes are supported (with more to come as necessary): Unkno The following error codes are supported (with more to come as necessary): Unknown, TF Error, Invalid Path, Patience Exceeded, Failed To Make Progress, or No Valid Control. +`PR #3251 `_ pipes the highest priority error code through the bt_navigator and defines the error code structure. + +A new parameter for the the BT Navigator called "error_code_id_names" was added to the nav2_params.yaml to define the error codes to compare. +The lowest error in the "error_code_id_names" is then returned in the action request (navigate to pose, navigate through poses waypoint follower), whereas the code enums increase the higher up in the software stack - giving higher priority to lower-level failures. + +The error codes produced from the servers follow the guidelines stated below. +Error codes from 0 to 9999 are reserved for nav2 while error codes from 10000-65535 are reserved for external servers. +Each server has two "reserved" error codes. 0 is reserved for NONE and the first error code in the sequence is reserved for UNKNOWN. + +The current implemented servers with error codes are: + +- Controller Server: NONE:0, UNKNOWN:100, server error codes: 101-199 +- Planner Server(compute_path_to_pose): NONE:0, UNKNOWN:201, server error codes: 201-299 +- Planner Server(compute_path_through_poses): NONE:0, UNKNOWN:301, server error codes: 301-399 +- Smoother Server: NONE: 0, UNKNOWN: 501, server error codes: 501-599 +- Waypoint Follower Server: NONE: 0, UNKNOWN: 601, server error codes: 601-699 + +This pr also updates the waypoint follower server to throw exceptions on failures. These exceptions get reported back to the server which in turn places a error code on the Behavior Tree Navigator's blackboard for use in contextual error handling in the autonomy application. +The following errors codes are supported (with more to come as necessary): Unknown and Task Executor Failed. +See :ref:`adding_a_nav2_task_server` and the PR for additional information. + Costmap Filters *************** @@ -86,6 +107,11 @@ SmootherSelector BT Node ************************ `PR #3283 `_ adds a BT node to set the smoother based on a topic or a default. See the configuration guide :ref:`SimpleSmoother` for more details. + +Publish Costmap Layers +********************** +`PR #3320 `_ adds the ability for the nav2_costmap_2d package to publish out costmap data associated with each layer. + Give Behavior Server Access to Both Costmaps ******************************************** `PR #3255 `_ addes the ability for a behavior to access the local and global costmap. From 3979ce8674cde991be1bd7113401c18dc6554ebb Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 13 Dec 2022 10:28:25 -0500 Subject: [PATCH 5/6] code review --- migration/Humble.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/migration/Humble.rst b/migration/Humble.rst index 40764aa98b..0dbf9bf5dd 100644 --- a/migration/Humble.rst +++ b/migration/Humble.rst @@ -118,4 +118,5 @@ Give Behavior Server Access to Both Costmaps To update behaviors, any reference to the global_frame must be updated to the local_frame parameter along with the ``configuration`` method which now takes in the local and global collision checkers. -Lastly, ``getResourceInfo`` must be overriden to return ``CostmapInfoType::LOCAL``. +Lastly, ``getResourceInfo`` must be overriden to return ``CostmapInfoType::LOCAL``. Other options include ``GLOBAL``if the behavior useses global costmap and/or footprint) +or ``BOTH`` if both are required. This allows us to only create and maintain the minimum amount of expensive resources. From 0430288d6d9b6d5842011214ec525842ca06ae38 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 13 Dec 2022 09:42:09 -0800 Subject: [PATCH 6/6] Update migration/Humble.rst --- migration/Humble.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/migration/Humble.rst b/migration/Humble.rst index 0dbf9bf5dd..97d0b48f24 100644 --- a/migration/Humble.rst +++ b/migration/Humble.rst @@ -118,5 +118,5 @@ Give Behavior Server Access to Both Costmaps To update behaviors, any reference to the global_frame must be updated to the local_frame parameter along with the ``configuration`` method which now takes in the local and global collision checkers. -Lastly, ``getResourceInfo`` must be overriden to return ``CostmapInfoType::LOCAL``. Other options include ``GLOBAL``if the behavior useses global costmap and/or footprint) +Lastly, ``getResourceInfo`` must be overriden to return ``CostmapInfoType::LOCAL``. Other options include ``GLOBAL`` if the behavior useses global costmap and/or footprint) or ``BOTH`` if both are required. This allows us to only create and maintain the minimum amount of expensive resources.