Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Document bt_navigator error_code_name_prefixes and error_msg #587

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Document bt_navigator error_code_name_prefixes and error_msg #587

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3deba65
Trim trailing whitespace
aosmw Aug 31, 2024
23e4b92
Document behavior tree error_code_name_prefixes, error_msg
aosmw Aug 31, 2024
ed98c63
address error_msg review comments
aosmw Sep 7, 2024
6b2c1af
Update migration/Jazzy.rst
SteveMacenski Sep 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions behavior_trees/trees/follow_point.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,13 +28,13 @@ This behavior tree will execute infinitely in time until the navigation request
<RateController hz="1.0">
<Sequence>
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
<ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
</GoalUpdater>
<TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
</Sequence>
</RateController>
<KeepRunningUntilFailure>
<FollowPath path="{truncated_path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
<FollowPath path="{truncated_path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
</KeepRunningUntilFailure>
</PipelineSequence>
</BehaviorTree>
Expand Down
8 changes: 4 additions & 4 deletions behavior_trees/trees/nav_through_poses_recovery.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
<ReactiveSequence>
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
</ReactiveSequence>
<Sequence>
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
Expand All @@ -56,7 +56,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
<Sequence>
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
Expand All @@ -75,9 +75,9 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Spin spin_dist="1.57" error_code_id="{spin_error_code}"/>
<Spin spin_dist="1.57" error_code_id="{spin_error_code} error_msg="{spin_error_msg}"/>
<Wait wait_duration="5.0"/>
<BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}"/>
<BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
</RoundRobin>
</ReactiveFallback>
</Sequence>
Expand Down
38 changes: 19 additions & 19 deletions behavior_trees/trees/nav_to_pose_and_pause_near_goal_obstacle.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,38 +1,38 @@
Navigate To Pose and Pause Near Goal-Obstacle
#############################################

.. note:: As a prerequisite, we encourage the users to go through the `Behavior Tree documentation <https://behaviortree.github.io/BehaviorTree.CPP/>`_, which explains about different behaviors nodes used in these trees such as ``ReactiveSequence``, ``SequenceStar`` and ``RetryUntilSucessfull``.
.. note:: As a prerequisite, we encourage the users to go through the `Behavior Tree documentation <https://behaviortree.github.io/BehaviorTree.CPP/>`_, which explains about different behaviors nodes used in these trees such as ``ReactiveSequence``, ``SequenceStar`` and ``RetryUntilSucessfull``.

This behavior tree is a soft extension to the :ref:`behavior_tree_nav_to_pose`.
This behavior tree is a soft extension to the :ref:`behavior_tree_nav_to_pose`.
Apart from the functionalities of :ref:`behavior_tree_nav_to_pose`, this behavior tree allows the robot to efficiently handle an obstacle (e.g. forklift, person, or other temporary obstacles) close to the goal by pausing the robot's navigation and wait for a user-specified time to check if the obstacle has cleared.
If the obstacle has moved during the waiting time, the robot will continue to the goal taking the shorter path. If the obstacle has not moved during the waiting time or the waiting time expires, then the robot will use the longer path around to reach the final goal location.
Ultimately, for a given task, this behavior tree aids in solving the problem of long cycle time, which is caused because of the long path generated due to the temporary obstacles present close to the goal location.
Ultimately, for a given task, this behavior tree aids in solving the problem of long cycle time, which is caused because of the long path generated due to the temporary obstacles present close to the goal location.

The behavior tree is depicted in the image below.
From the image, it can be noted that there is an additional branch in the Navigation Subtree known as ``MonitorAndFollowPath``. This branch is created with the intention for the users to perform any kind of monitoring behavior that their robot should exhibit.
In this particular BT, the monitoring branch is exclusively utilized by ``PathLongerOnApproach`` BT node for checking if the global planner has decided to plan a significantly longer path for the robot on approaching the user-specified goal proximity.
If there is no significantly longer path, the monitor node goes into the ``FollowPath`` recovery node, which then generates the necessary control commands.
The behavior tree is depicted in the image below.
From the image, it can be noted that there is an additional branch in the Navigation Subtree known as ``MonitorAndFollowPath``. This branch is created with the intention for the users to perform any kind of monitoring behavior that their robot should exhibit.
In this particular BT, the monitoring branch is exclusively utilized by ``PathLongerOnApproach`` BT node for checking if the global planner has decided to plan a significantly longer path for the robot on approaching the user-specified goal proximity.
If there is no significantly longer path, the monitor node goes into the ``FollowPath`` recovery node, which then generates the necessary control commands.

.. image:: ../images/walkthrough/patience_and_recovery.png

Once there is a significantly longer path, the child node for the ``PathLongerOnApproach`` node ticks.
The child node is a ``RetryUntilSuccesfull`` decorator node, which inturns have a ``SequenceStar`` node as its child.
Firstly, the ``SequenceStar`` node cancels the controller server by ticking the ``CancelControl`` node. The cancellation of the controller server halts the further navigation of the robot.
Next, the ``SequenceStar`` node ticks the ``Wait`` node, which enables the robot to wait for the given user-specified time.
The child node is a ``RetryUntilSuccesfull`` decorator node, which inturns have a ``SequenceStar`` node as its child.
Firstly, the ``SequenceStar`` node cancels the controller server by ticking the ``CancelControl`` node. The cancellation of the controller server halts the further navigation of the robot.
Next, the ``SequenceStar`` node ticks the ``Wait`` node, which enables the robot to wait for the given user-specified time.
Here we need to note that, the ``MonitorAndFollowPath`` is a ``ReactiveSequence`` node, therefore the ``PathLongerOnApproach`` node needs to return SUCCESS, before the ``FollowPath`` node can be ticked once again.

In the below GIF, it can be seen that the robot is approaching the goal location, but it found an obstacle in the goal proximity, because of which the global planner, plans a longer path around.
This is the point where the ``PathLongerOnApproach`` ticks and ticks its children, consequently cancelling the ``controller_server`` and waiting to see if the obstacle clears up.
In the below scenario, the obstacles do not clear, causing the robot to take the longer path.
In the below GIF, it can be seen that the robot is approaching the goal location, but it found an obstacle in the goal proximity, because of which the global planner, plans a longer path around.
This is the point where the ``PathLongerOnApproach`` ticks and ticks its children, consequently cancelling the ``controller_server`` and waiting to see if the obstacle clears up.
In the below scenario, the obstacles do not clear, causing the robot to take the longer path.

.. image:: ../../migration/images/nav2_patience_near_goal_and_go_around.gif

Alternatively, if the obstacles are cleared, then there is a shorter path generated by the global planner.
Alternatively, if the obstacles are cleared, then there is a shorter path generated by the global planner.
Now, the ``PathLongerOnApproach`` returns SUCCESS, that cause the ``FollowPath`` to continue with the robot navigation.

.. image:: ../../migration/images/nav2_patience_near_goal_and_clear_obstacle.gif

Apart from the above scenarios, we also need to note that, the robot will take the longer path to the goal location if the obstacle does not clear up in the given user-specific wait time.
Apart from the above scenarios, we also need to note that, the robot will take the longer path to the goal location if the obstacle does not clear up in the given user-specific wait time.

In conclusion, this particular BT would serve, both as an example and ready-to-use BT for an organizational specific application, that wishes to optimize its process cycle time.

Expand All @@ -46,7 +46,7 @@ In conclusion, this particular BT would serve, both as an example and ready-to-u
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
Expand All @@ -60,7 +60,7 @@ In conclusion, this particular BT would serve, both as an example and ready-to-u
</RetryUntilSuccessful>
</PathLongerOnApproach>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</ReactiveSequence>
Expand All @@ -72,9 +72,9 @@ In conclusion, this particular BT would serve, both as an example and ready-to-u
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Spin spin_dist="1.57"/>
<Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
<Wait wait_duration="5.0"/>
<BackUp backup_dist="0.30" backup_speed="0.05"/>
<BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
Expand Down
8 changes: 4 additions & 4 deletions behavior_trees/trees/nav_to_pose_recovery.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,15 +42,15 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
<Sequence>
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
<Sequence>
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
Expand All @@ -69,9 +69,9 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Spin spin_dist="1.57" error_code_id="{spin_error_code}"/>
<Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
<Wait wait_duration="5.0"/>
<BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_code_id}"/>
<BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
</RoundRobin>
</ReactiveFallback>
</Sequence>
Expand Down
8 changes: 4 additions & 4 deletions ...es/trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,13 +45,13 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
</Inverter>
<IsPathValid path="{path}"/>
</ReactiveSequence>
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
</Fallback>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="RecoveryFollowPath">
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This BT missing the spin/backup error_msg?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done, although, spin and backup are not in the default list of error_code_name_prefixes. Do you want them added along with all builtin actions?

  std::vector<std::string> error_code_name_prefixes = {
    "follow_path",
    "compute_path",
  };

https://github.com/ros-navigation/navigation2/blob/52e7a79cbc7adc9e9fc7634dc26cc064e46eab97/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp#L76C1-L80C1

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think its fine for them to not be in the defaults, as long as they're in the nav2_params.yaml that we ship by default. I think the absolute-defaults are the follow / compute paths

<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</PipelineSequence>
Expand All @@ -62,9 +62,9 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Spin name="SpinRecovery" spin_dist="1.57"/>
<Spin name="SpinRecovery" spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
<Wait name="WaitRecovery" wait_duration="5.0"/>
<BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05"/>
<BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
Expand Down
22 changes: 17 additions & 5 deletions configuration/packages/bt-plugins/actions/AssistedTeleop.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ Input Ports
====== =======

Description
Action server name.
Action server name.

:server_timeout:

Expand All @@ -55,22 +55,34 @@ Input Ports
====== =======

Description
Action server timeout (ms).
Action server timeout (ms).

:error_code_id:

============== =======
Type Default
-------------- -------
uint16 N/A
uint16 N/A
============== =======

Description
Assisted teleop error code. See ``AssistedTeleop`` action message for the enumerated set of error codes.
Assisted teleop error code. See ``AssistedTeleop`` action message for the enumerated set of error codes.

:error_msg:

============== =======
Type Default
-------------- -------
uint16 N/A
============== =======

Description
Assisted teleop error message. See ``AssistedTeleop`` action message for the enumerated set of error codes.

Example
-------

.. code-block:: xml

<AssistedTeleop is_recovery="false" server_name="assisted_teleop_server" server_timeout="10" error_code_id="{assisted_teleop_error_code}"/>
<AssistedTeleop is_recovery="false" server_name="assisted_teleop_server" server_timeout="10"
error_code_id="{assisted_teleop_error_code}" error_msg="{assisted_teleop_error_msg}"/>
26 changes: 19 additions & 7 deletions configuration/packages/bt-plugins/actions/BackUp.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ Input Ports
====== =======

Description
Total distance to backup (m).
Total distance to backup (m).

:backup_speed:

Expand All @@ -32,7 +32,7 @@ Input Ports
====== =======

Description
Backup speed (m/s).
Backup speed (m/s).

:time_allowance:

Expand All @@ -54,7 +54,7 @@ Input Ports
====== =======

Description
Action server name.
Action server name.

:server_timeout:

Expand All @@ -65,7 +65,7 @@ Input Ports
====== =======

Description
Action server timeout (ms).
Action server timeout (ms).

Output Ports
------------
Expand All @@ -75,15 +75,27 @@ Output Ports
============== =======
Type Default
-------------- -------
uint16 N/A
uint16 N/A
============== =======

Description
Backup error code. See ``BackUp`` action message for the enumerated set of error codes.
Backup error code. See ``BackUp`` action message for the enumerated set of error codes.

:error_msg:

============== =======
Type Default
-------------- -------
string N/A
============== =======

Description
Backup error message. See ``BackUp`` action message for the enumerated set of error codes.

Example
-------

.. code-block:: xml

<BackUp backup_dist="-0.2" backup_speed="0.05" server_name="backup_server" server_timeout="10" error_code_id="{backup_error_code}"/>
<BackUp backup_dist="-0.2" backup_speed="0.05" server_name="backup_server" server_timeout="10"
error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
Loading
Loading