diff --git a/commander_api/index.rst b/commander_api/index.rst index fbf1710c2f..6f4ea7a7c1 100644 --- a/commander_api/index.rst +++ b/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: diff --git a/concepts/index.rst b/concepts/index.rst index 4c46b75d22..220434f9de 100644 --- a/concepts/index.rst +++ b/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 ================ diff --git a/configuration/index.rst b/configuration/index.rst index aaff8e8ad3..1679d21bd0 100644 --- a/configuration/index.rst +++ b/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 diff --git a/configuration/packages/configuring-route-server.rst b/configuration/packages/configuring-route-server.rst new file mode 100644 index 0000000000..be032c2345 --- /dev/null +++ b/configuration/packages/configuring-route-server.rst @@ -0,0 +1,661 @@ +.. _configuring_croute_server: + +Route Server +############ + +Source code on Github_. + +.. _Github: https://github.com/ros-navigation/navigation2/tree/main/nav2_route + +The Route Server in ``nav2_route`` implements the server for computing routes through a predefined navigation graph rather than using freespace planning like the Planner Server. +It may be used to fully replace freespace planning when following a particular route closely or to augment the global planner with long-distance routing to a goal. +In this case, the planner will generate feasible paths with localized environmental information for only the future part of the route necessary. + +The graph itself has very few rules associated with it, but may be generated manually or automatically via AI, geometric, or probabilistic techniques. +See :ref:`route_graph_generation` for a tutorial about how to generate a graph using QGIS. + +There are also several locations for customization using plugins: +* Edge Scorer: Enables custom scoring functions for edges based on arbitrary user-defined semantic information and the chosen optimization criteria(s). +* Route Operation: Execute a user-defined action when entering or leaving a route edge or achieving a node, including use of node and edge semantic information, +* Route Graph Parser: Parse graph files using the chosen format (e.g. geoJSON, OpenStreetMap, etc.) and convert them into the internal graph representation. + +See the package's README file for additional information such as performance metrics, design, advice, and graph formatting. + + +Server Parameters +***************** + +:base_frame: + + ============== ============== + Type Default + -------------- -------------- + string "base_link" + ============== ============== + + Description + The base frame of the robot to use to obtain the robot's pose from when not using the ``use_start`` request parameter. + +:route_frame: + + ============== ============== + Type Default + -------------- -------------- + string "map" + ============== ============== + + Description + The frame of the route graph to plan within. If values in the graph file are not w.r.t. this frame, they will be automatically transformed. + + +:path_density: + + ============== ======== + Type Default + -------------- -------- + double 0.05 + ============== ======== + + Description + The density of path-points in the output route, if using the ``nav_msgs/Path`` route rather than the collection of nodes and edges. This is used to upsample the route into a path that may be followed. + +:max_iterations: + + ============== ======== + Type Default + -------------- -------- + int 0 + ============== ======== + + Description + The maximum number of planning iterations to perform. If 0, the maximum number of iterations is used. + +:max_planning_time: + + ============== ======== + Type Default + -------------- -------- + double 2.0 + ============== ======== + + Description + The maximum planning time to use. + +:tracker_update_rate: + + ============== ======== + Type Default + -------------- -------- + double 50.0 + ============== ======== + + Description + The update rate of the tracker (when using ``ComputeAndTrackRoute`` action) to check the status of path tracking and execute route operations. + +:aggregate_blocked_ids: + + ============== ======== + Type Default + -------------- -------- + bool false + ============== ======== + + Description + Whether to aggregate the blocked IDs reported by route operations over the lifespan of the navigation request or only use the currently marked blocked IDs. + +:boundary_radius_to_achieve_node: + + ============== ======== + Type Default + -------------- -------- + double 1.0 + ============== ======== + + Description + The radius at a boundary condition (start, goal) to mark the node achieved by the tracker when using ``ComputeAndTrackRoute``. Note that this is not the same as the goal tolerance, as the route or path follower (controller) will continue to run until its goal tolerance is met. + +:radius_to_achieve_node: + + ============== ========== + Type Default + -------------- ---------- + double 1.0 + ============== ========== + + Description + The radius for non-boundary conditions to mark the node as achieved once within tolerance of, when using ``ComputeAndTrackRoute``. Note that this is a radius to consider achievable, however a refinement process takes place to most accurately identify when a node is reached. The radius is the trigger to start this process. Set this generously based on path tracking tolerances. + +:max_prune_dist_from_edge: + + ============== ========== + Type Default + -------------- ---------- + double 8.0 + ============== ========== + + Description + Maximum distance from an edge to consider pruning it as in-progress (i.e. if we're to far away from an edge, it is nonsensical to prune it). + +:min_prune_dist_from_goal: + + ============== =============== + Type Default + -------------- --------------- + double 0.15 + ============== =============== + + Description + Minimum distance from the goal node away from the request's goal pose (if using ``use_poses``) to consider pruning as being passed, in case the goal pose is very close to the goal node, but is not exact. + +:min_prune_dist_from_start: + + ============== =============== + Type Default + -------------- --------------- + double 0.1 + ============== =============== + + Description + Minimum distance from the start node away from the start pose (if using ``use_poses``) to consider pruning as being passed, in case the start pose is very close to the start node, but is not exact. + +:prune_goal: + + ============== =============== + Type Default + -------------- --------------- + bool true + ============== =============== + + Description + Whether pruning the goal node from the route due to it being spatially past the goal pose requested (pose requests only ``use_poses``). + +:graph_filepath: + + ============== =============== + Type Default + -------------- --------------- + string "" + ============== =============== + + Description + The filepath to the graph file for loading. It may be empty on initialization, but then the graph must be set from the server's set graph service later. + +:graph_file_loader: + + ============== ======================== + Type Default + -------------- ------------------------ + string "GeoJsonGraphFileLoader" + ============== ======================== + + Description + The name of the graph file loader plugin to use. + +:graph_file_loader.plugin: + + ============== ==================================== + Type Default + -------------- ------------------------------------ + string "nav2_route::GeoJsonGraphFileLoader" + ============== ==================================== + + Description + The graph loading plugin to use. By default, we use ``geojson``. + +:edge_cost_functions: + + ============== ======================================== + Type Default + -------------- ---------------------------------------- + vector ["DistanceScorer", "DynamicEdgesScorer"] + ============== ======================================== + + Description + Which edge cost functions should be used for planning purposes to score the edges. By default, we optimize for minimum distance while providing a service cost function to set arbitrary costs or mark edge as closed from a service. + +:operations: + + ============== ======================================== + Type Default + -------------- ---------------------------------------- + vector ["AdjustSpeedLimit", "ReroutingService"] + ============== ======================================== + + Description + The route operation plugins to use for ``ComputeAndTrackRoute``. By default, we have a speed limit adjuster and a ROS service request rerouting operation. + +:.plugin: + + ============== ============ + Type Default + -------------- ------------ + string "" + ============== ============ + + Description + The plugin to load under that name. The ``edge_cost_functions.`` namespaces is also where plugin-specific parameters are defined. + + +Edge Scorer Parameters +********************** + +DistanceScorer +-------------- + +This edge scoring plugin will score based on the length of the edge. +If a ``speed_tag`` is provided, that is used to scale by the time to traverse. +This must be a percentage, if using absolute speed limits, see the ``TimeScorer`` plugin below. + +:weight: + + ============== ============ + Type Default + -------------- ------------ + double 1.0 + ============== ============ + + Description + Relative edge scoring weighting. + +:speed_tag: + + ============== ============ + Type Default + -------------- ------------ + string "speed_limit" + ============== ============ + + Description + Graph metadata key to look for percentage speed limits (speed_limit). + + +TimeScorer +---------- + +This edge scoring plugin will score based on the time to traverse the length of the edge. +This will use the distance of the edge weighted in proportion to the absolute speed limits of the robot over an edge. +If none is set in the graph, a parameterized maximum speed will be used. +If an actual, measured time of a previous traversal is in the edge's metadata, this will be used. + +:weight: + + ============== ============ + Type Default + -------------- ------------ + double 1.0 + ============== ============ + + Description + Relative edge scoring weighting. + +:speed_tag: + + ============== ================= + Type Default + -------------- ----------------- + string "abs_speed_limit" + ============== ================= + + Description + Graph metadata key to look for absolute speed limits. + +:time_tag: + + ============== ================ + Type Default + -------------- ---------------- + string "abs_time_taken" + ============== ================ + + Description + Graph metadata key to look for abs traversal times. + +:max_vel: + + ============== ================ + Type Default + -------------- ---------------- + double 0.5 + ============== ================ + + Description + Maximum velocity to use if speed limit or time taken is not set. + +PenaltyScorer +------------- + +This edge scoring plugin will score based on a statically set penalty in the graph file for a particular edge. +This can be based on application known logic to weight preferences of navigation tactics in a space. + +:weight: + + ============== ============ + Type Default + -------------- ------------ + double 1.0 + ============== ============ + + Description + Relative edge scoring weighting. + +:penalty_tag: + + ============== ================ + Type Default + -------------- ---------------- + string "penalty" + ============== ================ + + Description + Graph metadata key to look for penalty value. + +SemanticScorer +-------------- + +This edge scoring plugin will score based on semantic information provided in the graph file. +It can either check for the edge's semantic class via a parameterized key's value **or** search all key names to match known semantic classes to apply weight (e.g. `class: highway` or `highway: `). + + +:weight: + + ============== ============ + Type Default + -------------- ------------ + double 1.0 + ============== ============ + + Description + Relative edge scoring weighting. + +:semantic_classes: + + ============== ============ + Type Default + -------------- ------------ + vector [] + ============== ============ + + Description + The list of semantic classes in your graph that you would like to score based off of. + +:: + + ============== ============ + Type Default + -------------- ------------ + double N/A + ============== ============ + + Description + The cost to assign to this semantic class. For example: ``highway: 8.4``. + +:semantic_key: + + ============== ============ + Type Default + -------------- ------------ + string class + ============== ============ + + Description + The key to search for edge's semantic data with the edge's metadata. If empty string, will look at key names instead. + +StartPoseOrientationScorer +-------------------------- + +This edge scoring plugin will score an edge starting at the start node (vector from start->goal) based on its angular proximity to the starting pose's orientation. +This will either score a weighted-angular distance or reject traversals that are outside of a set threshold to force the route to go down a particular direction (i.e. direction robot is already facing). + +:orientation_weight: + + ============== ============ + Type Default + -------------- ------------ + double 1.0 + ============== ============ + + Description + Relative edge scoring weighting. + +:use_orientation_threshold: + + ============== ============ + Type Default + -------------- ------------ + bool false + ============== ============ + + Description + Whether to use the orientation threshold for binary validity of traversal or weighted-angular distance scoring. + +:orientation_tolerance: + + ============== ============ + Type Default + -------------- ------------ + double PI/2 + ============== ============ + + Description + The angular threshold to reject edges' angles if greater than this w.r.t. starting pose, when ``use_orientation_threshold: true``. + + +GoalPoseOrientationScorer +------------------------- + +This edge scoring plugin will score a an edge with terminus of the goal node (vector from start->goal) based on its angular proximity to the goal pose's orientation. +This will either score a weighted-angular distance or reject traversals that are outside of a set threshold to force the route to go down a particular direction (i.e. direction robot wants to be facing). + +:orientation_weight: + + ============== ============ + Type Default + -------------- ------------ + double 1.0 + ============== ============ + + Description + Relative edge scoring weighting. + +:use_orientation_threshold: + + ============== ============ + Type Default + -------------- ------------ + bool false + ============== ============ + + Description + Whether to use the orientation threshold for binary validity of traversal or weighted-angular distance scoring. + +:orientation_tolerance: + + ============== ============ + Type Default + -------------- ------------ + double PI/2 + ============== ============ + + Description + The angular threshold to reject edges' angles if greater than this w.r.t. goal pose, when ``use_orientation_threshold: true``. + + +DynamicEdgesScorer +------------------ + +This edge scoring plugin will score based on the requested values from a 3rd party application via a service interface. +It can set dynamically any cost for any edge and also be used to close and reopen particular edges if they are blocked, in use by other robots locking out its shared use by other robots, higher cost due to overlap with other platforms in service, increased cost due to fleet manager analytics that this space is underperforming throughput, or otherwise temporarily non-traversible. +For example, if other robots report an edge to be blocked, all robots can avoid this edge/aisle/etc. + +It has no parameters. + +Route Operations Parameters +*************************** + +AdjustSpeedLimit +---------------- + +This route operation will check the graph at each state change (e.g. node passed) if the new edge entered contains speed limit restrictions. If so, it will publish those to the speed limit topic to be received by the controller server. + +:speed_limit_topic: + + ============== ============ + Type Default + -------------- ------------ + string speed_limit + ============== ============ + + Description + The topic to publish new speed limits to. + +:speed_tag: + + ============== ============ + Type Default + -------------- ------------ + string speed_limit + ============== ============ + + Description + The graph's semantic metadata key to look for speed limits under. + + +CollisionMonitor +---------------- + +This route operation will evaluate a future-looking portion of the route for validity w.r.t. collision in the costmap. +If it is blocked, it sets the edge blocked as blocked for rerouting around the blocked edge or fail the action based on ``reroute_on_collision``. + +:costmap_topic: + + ============== ============================ + Type Default + -------------- ---------------------------- + string "global_costmap/costmap_raw" + ============== ============================ + + Description + The costmap topic to use for collision checking. May be local or global costmap depending on the desired collision checking horizon. + +:rate: + + ============== ============================ + Type Default + -------------- ---------------------------- + double 1.0 + ============== ============================ + + Description + The rate to collision at, rather than the tracker's update rate since this is an expensive operation. + +:max_cost: + + ============== ============================ + Type Default + -------------- ---------------------------- + double 253.0 + ============== ============================ + + Description + The cost at or above which is considered invalid. + +:max_collision_dist: + + ============== ============================ + Type Default + -------------- ---------------------------- + double 5.0 + ============== ============================ + + Description + The distance (meters) ahead of the robot's position on the route to collision check during. + +:check_resolution: + + ============== ============================ + Type Default + -------------- ---------------------------- + int 1 + ============== ============================ + + Description + The resolution to check at in terms of multiples of the costmap's resolution (1 = 1 cell, 2 = every 2 cells, and so on). This reduces the computational complexity for long-range routes. + +:reroute_on_collision: + + ============== ============================ + Type Default + -------------- ---------------------------- + bool true + ============== ============================ + + Description + Whether to reroute on collision or exit the tracking task as a failure when future collision is detected. + + +TimeMarker +---------- + +This route operation will track times taken to traverse particular edges to write times to for later improved navigation time estimation in edge scoring. + +:time_tag: + + ============== ============================ + Type Default + -------------- ---------------------------- + string abs_time_taken + ============== ============================ + + Description + Metadata tag to write the time taken to within an edge. Is used with the ``TimeScorer`` to give an improved experiential estimate of traversal times. + + +ReroutingService +---------------- + +This route operation will receive service requests from a 3rd party application to cause a rerouting request. + +TriggerEvent +------------ + +This route operation will trigger an external service when a graph node or edge contains a route operation of this name. +It uses a `std_srvs/Trigger` interface and is a demonstration of the `RouteOperationClient` base class which can be used to trigger other events of other types of other names as desired (opening doors, calling elevators, etc). + +Example +******* +.. code-block:: yaml + + route_server: + ros__parameters: + + base_frame: "base_link" # Robot's base frame + route_frame: "map" # Global reference frame + path_density: 0.05 # Density of points for generating the dense nav_msgs/Path from route (m) + max_iterations: 0 # Maximum number of search iterations, if 0, uses maximum possible + max_planning_time: 2.0 # Maximum planning time (seconds) + + graph_file_loader: "GeoJsonGraphFileLoader" # Name of default file loader + plugin: nav2_route::GeoJsonGraphFileLoader # file loader plugin to use + graph_filepath: "" # file path to graph to use + + edge_cost_functions: ["DistanceScorer", "DynamicEdgesScorer"] # Edge scoring cost functions to use + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + DynamicEdgesScorer: + plugin: "nav2_route::DynamicEdgesScorer" + + operations: ["AdjustSpeedLimit", "ReroutingService"] # Route operations plugins to use + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + ReroutingService: + plugin: "nav2_route::ReroutingService" + + tracker_update_rate: 50.0 # Rate at which to check the status of path tracking + aggregate_blocked_ids: false # Whether to aggregate the blocked IDs reported by route operations over the lifespan of the navigation request or only use the currently blocked IDs. + boundary_radius_to_achieve_node: 1.0 # Radius (m) near boundary nodes (e.g. start/end) to enable evaluation of achievement metric + radius_to_achieve_node: 2.0 # Radius (m) near route nodes as preliminary condition for evaluation of achievement metric + + max_prune_dist_from_edge: 8.0 # Max distance from an edge to consider pruning it as in-progress (e.g. if we're too far away from the edge, its nonsensical to prune it) + min_prune_dist_from_goal: 0.15 # Min distance from goal node away from goal pose to consider goal node pruning as considering it as being passed (in case goal pose is very close to a goal node, but not exact) + min_prune_dist_from_start: 0.10 # Min distance from start node away from start pose to consider start node pruning as considering it as being passed (in case start pose is very close to a start node, but not exact) + prune_goal: true # Whether pruning the goal nodes from the route due to being past the goal pose requested is possible (pose requests only) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index e37c854ce6..2ddd8d09a4 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -5,6 +5,20 @@ Jazzy to Kilted Moving from ROS 2 Jazzy to Kilted, a number of stability improvements were added that we will not specifically address here. +Nav2 Route Server +***************** + +The Route Server in ``nav2_route`` implements the server for computing routes through a predefined navigation graph rather than using freespace planning like the Planner Server. +It may be used to fully replace freespace planning when following a particular route closely or to augment the global planner with long-distance routing to a goal. +In this case, the planner will generate feasible paths with localized environmental information for only the future part of the route necessary. + +This is useful for industrial applications where the robot should deterministically plan within known areas, lanes, and/or routes rather than having free rein to globally navigate. +It is also useful for outdoor navigation in locations like urban centers on roadways or natural environments over vast distances. + +It also includes a tracking feature that will track the route's progression and provide live feedback on its status as well as trigger contextual operations at various nodes and edge events (change speed, turn on light, etc). + +See :ref:`configuring_croute_server` for additional configuration information, the tutorials on generating graphs and using it, and example graphs in ``nav2_bringup`` and ``nav2_route``. + BehaviorTree error_msg ********************** diff --git a/plugins/index.rst b/plugins/index.rst index 5f7253aadb..b297803cd7 100644 --- a/plugins/index.rst +++ b/plugins/index.rst @@ -619,3 +619,66 @@ Behavior Tree Nodes .. _Pipeline Sequence: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp .. _Recovery: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/control/recovery_node.cpp .. _Round Robin: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/control/round_robin_node.cpp + + + +Route Plugins +============= + +Edge Scorers +------------ + ++--------------------------------+------------------------+----------------------------------+ +| Plugin Name | Creator | Description | ++================================+========================+==================================+ +| DistanceScorer | Steve Macenski | Scores an edge's length, | +| | | optionally scaled by relative | +| | | speed limits. | ++--------------------------------+------------------------+----------------------------------+ +| TimeScorer | Steve Macenski | Scores and edge traversal time | +| | | using absolute speed limits or | +| | | previous traversal times. | ++--------------------------------+------------------------+----------------------------------+ +| PenaltyScorer | Steve Macenski | Scores using a static semantic | +| | | penalty. | ++--------------------------------+------------------------+----------------------------------+ +| SemanticScorer | Steve Macenski | Scores using stored semantic data| +| | | regarding the edge and/or nodes. | ++--------------------------------+------------------------+----------------------------------+ +| StartPoseOrientationScorer | Alex Yuen | Scores based on the initial pose | +| | | and start edge orientations. | ++--------------------------------+------------------------+----------------------------------+ +| GoalPoseOrientationScorer | Alex Yuen | Scores based on the goal pose and| +| | | goal edge orientations. | ++--------------------------------+------------------------+----------------------------------+ +| DynamicEdgesScorer | Steve Macenski | Scores based on a dynamically set| +| | | service cost and/or closure. | ++--------------------------------+------------------------+----------------------------------+ + +Route Operations +---------------- + ++--------------------------------+------------------------+----------------------------------+ +| Plugin Name | Creator | Description | ++================================+========================+==================================+ +| AdjustSpeedLimit | Steve Macenski | Adjusts robot speed limits using | +| | | an edge's semantic data. | ++--------------------------------+------------------------+----------------------------------+ +| CollisionMoniter | Steve Macenski | Checks for collision in the | +| | | immediate future which tracking | +| | | a route. | ++--------------------------------+------------------------+----------------------------------+ +| TimeMarker | Steve Macenski | Records the traversal time for an| +| | | edge in the edge's metadata. | ++--------------------------------+------------------------+----------------------------------+ +| ReroutingService | Steve Macenski | Triggers a rereoute from an | +| | | external server. | ++--------------------------------+------------------------+----------------------------------+ +| TriggerEvent | Steve Macenski | Triggers an event based on a | +| | | configurable server name. | ++--------------------------------+------------------------+----------------------------------+ + +Graph File Parsers +------------------ + +Currently, only ``geojson`` parsing is supported.