Merge branch 'master' into groot2

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

Author: ajtudela

behavior_trees/trees/nav_through_poses_recovery.rst

@@ -17,6 +17,7 @@ In nominal execution, this will replan the path at every 3 seconds and pass that
 The planner though is now ``ComputePathThroughPoses`` taking a vector, ``goals``, rather than a single pose ``goal`` to plan to.
 The ``RemovePassedGoals`` node is used to cull out ``goals`` that the robot has passed on its path.
 In this case, it is set to remove a pose from the poses when the robot is within ``0.5`` of the goal and it is the next goal in the list.
+Additionally, it records the status of each waypoint (e.g. ``PENDING``, ``COMPLETED``, ``SKIPPED`` or ``FAILED``) in the ``waypoint_statuses``.
 This is implemented such that replanning can be computed after the robot has passed by some of the intermediary poses and not continue to try to replan through them in the future.
 This time, if the planner fails, it will trigger contextually aware recoveries in its subtree, clearing the global costmap.
 Additional recoveries can be added here for additional context-specific recoveries, such as trying another algorithm.
@@ -46,7 +47,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
           <RateController hz="0.333">
             <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
               <ReactiveSequence>
-                <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
+                <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
                 <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
               </ReactiveSequence>
               <Sequence>

commander_api/index.rst

@@ -89,12 +89,12 @@ New as of September 2023: the simple navigator constructor will accept a `namesp
 +---------------------------------------+----------------------------------------------------------------------------+
 | cancelTask()                          | Cancel an ongoing task, including route tasks.                             |
 +---------------------------------------+----------------------------------------------------------------------------+
-| isTaskComplete(trackingRoute=False)   | Checks if task is complete yet, times out at ``100ms``. Returns            |
-|                                       | ``True`` if completed and ``False`` if still going. If checking a route    |
-|                                       | tracking task, set default argument to ``True``.                           |
+| isTaskComplete(task=RunningTask.NONE) | Checks if task is complete yet, times out at ``100ms``. Returns            |
+|                                       | ``True`` if completed and ``False`` if still going. Provide the task ID    |
+|                                       | from the long-running task (follow path, compute and track route, etc)     |
 +---------------------------------------+----------------------------------------------------------------------------+
-| getFeedback(trackingRoute=False)      | Gets feedback from task, returns action server feedback msg.               |
-|                                       | If getting feedback on a tracking task, set default argument to ``True``.  |
+| getFeedback(task=RunningTask.NONE)    | Gets feedback from task, returns action server feedback msg.               |
+|                                       | provide the task ID for the task you are requesting.                       |
 +---------------------------------------+----------------------------------------------------------------------------+
 | getResult()                           | Gets final result of task, to be called after ``isTaskComplete``           |
 |                                       | returns ``True``. Returns action server result msg.                        |
@@ -105,6 +105,12 @@ New as of September 2023: the simple navigator constructor will accept a `namesp
 | getPathThroughPoses(start, goals,     | Gets a path through a starting to a set of goals, a list                   |
 | planner_id='', use_start=False)       | of ``PoseStamped``, ``nav_msgs/Path``.                                     |
 +---------------------------------------+----------------------------------------------------------------------------+
+| getRoute(start, goal,                 | Gets a route from a set start and goal nodeIDs or PoseStamped.             |
+| use_start=False)                      | Use Start if given, otherwises uses TF to obtain robot pose.               |
++---------------------------------------+----------------------------------------------------------------------------+
+| getAndTrackRoute(start, goal,         | Gets a route from a set of start and goal NodeIDs or PoseStamped.          |
+| use_start=False)                      | Uses start if given, otherwise uses TF to obtain the robot pose.           |
++---------------------------------------+----------------------------------------------------------------------------+
 | dockRobot(dock_pose, dock_type)       | Attempts to dock the robot at a given docking pose and type, without using |
 |                                       | docking database of known docks.                                           |
 +---------------------------------------+----------------------------------------------------------------------------+
@@ -231,6 +237,7 @@ The ``nav2_simple_commander`` has a few examples to highlight the API functions
 - ``example_waypoint_follower.py`` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods.
 - ``example_follow_path.py`` - Demonstrates the path following capabilities of the navigator, as well as a number of auxiliary methods like path smoothing.
 - ``example_assisted_teleop.py`` - Demonstrates the assisted teleop capabilities of the navigator.
+- ``example_route.py`` - Demonstrates the route server capabilities of the navigator.
 
 The ``nav2_simple_commander`` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the API:
 

concepts/index.rst

@@ -107,9 +107,10 @@ Recoveries are used to get the robot out of a bad situation or attempt to deal w
 Smoothers can be used for additional quality improvements of the planned path.
 In this section, the general concepts around them and their uses in this project are analyzed.
 
-Planner, Controller, Smoother and Recovery Servers
-==================================================
-Four of the action servers in this project are the planner, behavior, smoother and controller servers.
+Planner, Controller, Smoother, Route, and Behavior Servers
+==========================================================
+
+Four of the action servers in this project are the planner, behavior, smoother, route, and controller servers.
 
 These action servers are used to host a map of algorithm plugins to complete various tasks.
 They also host the environmental representation used by the algorithm plugins to compute their outputs.
@@ -131,6 +132,10 @@ This is done because of the wide variety of behavior actions that may be created
 The behavior server also contains a costmap subscriber to the local costmap, receiving real-time updates from the controller server, to compute its tasks.
 We do this to avoid having multiple instances of the local costmap which are computationally expensive to duplicate.
 
+The route server does not contain multiple "routing algorithms" like the planner or controller servers.
+Instead, it computes a route using a navigation graph using a set of plugins for scoring edges in the graph, parsing graph files, and performing operations along the route, if necessary.
+Rather than freespace planning, this computes a route using a graph that can be generated to represent lanes, areas the robot is allowed to navigate, a teach-and-repeat route, urban roadways, and more.
+
 Alternatively, since the BT nodes are trivial plugins calling an action, new BT nodes can be created to call other action servers with other action types.
 It is advisable to use the provided servers if possible at all times.
 If, due to the plugin or action interfaces, a new server is needed, that can be sustained with the framework.
@@ -173,7 +178,7 @@ However, many classes of controllers and local planners exist.
 It is the goal of this project that all controller algorithms can be plugins in this server for common research and industrial tasks.
 
 Behaviors
-==========
+=========
 
 Recovery behaviors are a mainstay of fault-tolerant systems.
 The goal of recoveries are to deal with unknown or failure conditions of the system and autonomously handle them.
@@ -200,6 +205,13 @@ Use of a separate smoother over one that is included as part of a planner is adv
 The general task in Nav2 for a smoother is to receive a path and return its improved version.
 However, for different input paths, criteria of the improvements and methods of acquiring them exist, creating space for a multitude of smoothers that can be registered in this server.
 
+Route
+=====
+
+The route server is a specialized planner that computes a route using a navigation graph, rather than the freespace costmap.
+The route is computed as the optimal way from the start to the goal through the set of nodes and directional edges in the pre-defined navigation graph.
+This navigation graph can be generated to represent lanes, areas the robot is allowed to navigate, a teach-and-repeat route, urban roadways, and more.
+
 Robot Footprints
 ================
 

configuration/index.rst

@@ -36,3 +36,4 @@ the best navigation performance.
    packages/configuring-waypoint-follower.rst
    packages/configuring-loopback-sim.rst
    packages/configuring-docking-server.rst
+   packages/configuring-route-server.rst

configuration/packages/bt-plugins/actions/CancelComputeAndTrackRoute.rst

@@ -0,0 +1,39 @@
+.. _bt_cancel_compute_and_track_route:
+
+CancelComputeAndTrackRoute
+==========================
+
+Used to cancel the compute and track route 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 ``compute_and_track_route`` due to remapping.
+
+
+:server_timeout:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  double 10
+  ====== =======
+
+  Description
+      Server timeout (ms).
+
+Example
+-------
+
+.. code-block:: xml
+
+  <CancelComputeAndTrackRoute server_name="compute_and_track_route" server_timeout="10"/>

configuration/packages/bt-plugins/actions/ComputeAndTrackRoute.rst

@@ -0,0 +1,144 @@
+.. _bt_compute_and_track_route_action:
+
+ComputeAndTrackRoute
+====================
+
+Invokes the ComputeAndTrackRoute ROS 2 action server, which is implemented by the nav2_route_ module.
+The server address can be remapped using the ``server_name`` input port.
+
+.. _nav2_route: https://github.com/ros-navigation/navigation2/tree/main/nav2_route
+
+Input Ports
+-----------
+:start:
+
+  ===================================== =======
+  Type                                  Default
+  ------------------------------------- -------
+  geometry_msgs::msg::PoseStamped         N/A
+  ===================================== =======
+
+  Description
+        Start pose. Optional. Only used if not left empty. Takes in a blackboard variable, e.g. "{start}".
+
+:goal:
+
+  ===================================== =======
+  Type                                  Default
+  ------------------------------------- -------
+  geometry_msgs::msg::PoseStamped         N/A
+  ===================================== =======
+
+  Description
+        Goal pose. Takes in a blackboard variable, e.g. "{goal}".
+
+:start_id:
+
+  ===================================== =======
+  Type                                  Default
+  ------------------------------------- -------
+  int                                   N/A
+  ===================================== =======
+
+  Description
+        Start node ID to use.
+
+:goal_id:
+
+  ===================================== =======
+  Type                                  Default
+  ------------------------------------- -------
+  int                                   N/A
+  ===================================== =======
+
+  Description
+        Goal node ID to use.
+
+:use_start:
+
+  ============== =======
+  Type           Default
+  -------------- -------
+  bool           false
+  ============== =======
+
+  Description
+        Whether to use the start or use TF to obtain the robot's start pose.
+
+:use_poses:
+
+  ============== =======
+  Type           Default
+  -------------- -------
+  bool           false
+  ============== =======
+
+  Description
+        Whether to use the start and goal poses or start and goal node IDs.
+
+:server_name:
+
+  ============== =======
+  Type           Default
+  -------------- -------
+  string         N/A
+  ============== =======
+
+  Description
+        Action server name.
+
+
+:server_timeout:
+
+  ============== =======
+  Type           Default
+  -------------- -------
+  double         10
+  ============== =======
+
+  Description
+        Action server timeout (ms).
+
+Output Ports
+------------
+
+:execution_time:
+
+  ================================= =======
+  Type                              Default
+  --------------------------------- -------
+  builtin_interfaces::msg::Duration N/A
+  ================================= =======
+
+  Description
+        Time it took to compute the route.
+
+:error_code_id:
+
+  ============== =======
+  Type           Default
+  -------------- -------
+  uint16          N/A
+  ============== =======
+
+  Description
+        Compute route error code. See ``ComputeAndTrackRoute`` action message for the enumerated set of error codes.
+
+:error_msg:
+
+  ============== =======
+  Type           Default
+  -------------- -------
+  string         N/A
+  ============== =======
+
+  Description
+        Compute route error message. See ``ComputeAndTrackRoute`` action message for the enumerated set of error codes.
+
+Example
+-------
+
+.. code-block:: xml
+
+  <ComputeAndTrackRoute start="{start}" goal="{goal}" use_poses="{true}" use_start="{true}" server_name="ComputeAndTrackRoute" server_timeout="10"
+                     error_code_id="{compute_route_error_code}" error_msg="{compute_route_error_msg}"/>