diff --git a/nav2_amcl/README.md b/nav2_amcl/README.md index 29e6032696..7215500919 100644 --- a/nav2_amcl/README.md +++ b/nav2_amcl/README.md @@ -1,19 +1,4 @@ # AMCL -Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map. +Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map using a 2D laser scanner. This is largely a refactored port from ROS 1 without any algorithmic changes. -## Overview -Currently, the AMCL module in ROS 2 Navigation System is a direct port from [ROS1 AMCL](http://wiki.ros.org/amcl) package with some minor code re-factoring. The direct port includes all of ROS1 functionalities except running from Bag files. - -## Added Feature -AutoLocalization - is implemented by utilizing AMCL's `global_localization` service request and Behavior Tree (BT). This enables initial pose estimation capability on differential type robot. - - On startup of the navigation stack, the initial robot pose needs to be sent to AMCL otherwise AMCL initializes its filter state to default values with a particle cloud centered around (0,0,0). If the initial pose of the robot is not known and the robot is outside of default particle cloud, particles may not converge when robot moves. - -With the AutoLocalization branch of BT, first the `global_localization` service of AMCL is called to randomly disperse all particles through the free space in the map. Once particles are dispersed, the robot rotates, backs up, and, if necessary, rotates again to localize itself. The full implementation is described in the AutoLocalization section of [BT Navigator](https://github.com/ros-planning/navigation2/blob/main/nav2_bt_navigator/README.md) - -**Warning**: AutoLocalization actuates robot; currently, obstacle avoidance has not been integrated into this feature. The user is advised to not use this feature on a physical robot for safety reasons. As of now, this feature should only be used in simulations. - -## Future Plan -* Running from Ros bag -* Extending AMCL to work with different type of Sensors -* Improving overall localization performance with fusing data from different sensors such as IMU, Sonar, Radar, Laser, Depth camera, and etc. +See the [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings. diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 4d74a901f0..db761cb31d 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -1,11 +1,15 @@ # nav2_behavior_tree +This module is used by the nav2_bt_navigator to implement a ROS2 node that executes navigation Behavior Trees for either navigation or autonomy systems. The nav2_behavior_tree module uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) for the core Behavior Tree processing. + The nav2_behavior_tree module provides: -* A C++ template class for integrating ROS2 actions into Behavior Trees, +* A C++ template class for easily integrating ROS2 actions and services into Behavior Trees, * Navigation-specific behavior tree nodes, and -* a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes. +* a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes for navigation or higher-level autonomy applications. + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin. -This module is used by the nav2_bt_navigator to implement a ROS2 node that executes navigation Behavior Trees. The nav2_behavior_tree module uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) for the core Behavior Tree processing. +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. ## The bt_action_node Template and the Behavior Tree Engine @@ -59,37 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -## Navigation-Specific Behavior Tree Nodes - -The nav2_behavior_tree package provides several navigation-specific nodes that are pre-registered and can be included in Behavior Trees. - -| BT Node | Type | Description | -|----------|:-------------|------| -| Backup | Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. | -| ComputePathToPose | Action | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. The server address can be remapped using the `server_name` input port. | -| ComputePathThroughPoses | Action | Invokes the ComputePathThroughPoses ROS2 action server, which is implemented by the nav2_planner module. The server address can be remapped using the `server_name` input port. | -| RemovePassedGoals | Action | Removes `goals` from a vector of intermediary goal poses which the robot is either approaching or has passed within a tolerance of to prevent replanning to initial points. | -| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. The server address can be remapped using the `server_name` input port. | -| GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. | -| IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. | -| TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. | -| GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. | -| IsBatteryLow | Condition | Checks if battery is low by subscribing to a sensor_msgs/BatteryState topic and checking if battery voltage/percentage is below a specified minimum value. | -| NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. | -| NavigateThroughPoses | Action | Invokes the NavigateThroughPoses ROS2 action server, which is implemented by the bt_navigator module. | -| RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | -| DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | -| SpeedController | Decorator | A node that controls the tick rate for its child based on the current robot speed. This decorator offers the most flexibility as the user can set the minimum/maximum tick rate which is adjusted according to the current robot speed. | -| SingleTrigger | Decorator | A node that triggers its child node(s) only a single time. It returns `FAILURE` in all other ticks. | -| RecoveryNode | Control | The RecoveryNode is a control flow node with two children. It returns SUCCESS if and only if the first child returns SUCCESS. The second child will be executed only if the first child returns FAILURE. If the second child SUCCEEDS, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning FAILURE. In nav2, the RecoveryNode is included in Behavior Trees to implement recovery actions upon failures. -| Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is used in nav2 Behavior Trees as a recovery behavior. | -| PipelineSequence | Control | Ticks the first child till it succeeds, then ticks the first and second children till the second one succeeds. It then ticks the first, second, and third children until the third succeeds, and so on, and so on. If at any time a child returns RUNNING, that doesn't change the behavior. If at any time a child returns FAILURE, that stops all children and returns FAILURE overall.| -| ClearEntireCostmap | Action | Invokes the ClearEntireCostmap ROS2 service server of costmap_2d_ros. The service address needs to be specified using the `service_name` input port, typically `local_costmap/clear_entirely_local_costmap` or `global_costmap/clear_entirely_global_costmap`. This action is used in nav2 Behavior Trees as a recovery behavior. | -| ClearCostmapExceptRegion | Action | Invokes the ClearCostmapExceptRegion ROS2 service server of costmap_2d_ros. It will clear all the costmap apart a square area centered on the robot of side size equal to the `reset_distance` input port. The service address needs to be specified using the `service_name` input port, typically `local_costmap/clear_except_local_costmap` or `global_costmap/clear_except_global_costmap`. This action is used in nav2 Behavior Trees as a recovery behavior. | -| ClearCostmapAroundRobot | Action | Invokes the ClearCostmapAroundRobot ROS2 service server of costmap_2d_ros. It will only clear the costmap on a square area centered on the robot of side size equal to the `reset_distance` input port. The service address needs to be specified using the `service_name` input port, typically `local_costmap/clear_around_local_costmap` or `global_costmap/clear_around_global_costmap`. This action is used in nav2 Behavior Trees as a recovery behavior. | -| PlannerSelector | Action | The PlannerSelector behavior is used to switch the planner that will be used by the planner server. It subscribes to a topic "planner_selector" to get the decision about what planner must be used. It is usually used before of the ComputePathToPoseAction. The selected_planner output port is passed to planner_id input port of the ComputePathToPoseAction. -| ControllerSelector | Action | The ControllerSelector behavior is used to switch the controller that will be used by the controller server. It subscribes to a topic "controller_selector" to get the decision about what controller must be used. It is usually used before of the FollowPath. The selected_controller output port is passed to controller_id input port of the FollowPath. -| GoalCheckerSelector | Action | The GoalCheckerSelector behavior is used to switch the current goal checker of the controller_server. It subscribes to a topic "goal_checker_selector" to get the decision about what goal_checker must be used. It is usually used before of the FollowPath. The selected_goal_checker output port is passed to goal_checker_id input port of the FollowPath. - - For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/ diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index b2fd44434d..b47ef57ff9 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -6,158 +6,13 @@ This is a very flexible example for nav2 bringup that can be modified for differ Usual robot stacks will have a `_nav` package with config/bringup files and this is that for the general case to base a specific robot system off of. -Composed bringup (based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html)) is optional for users. It can be used to compose all Nav2 nodes in a single process instead of launching these nodes separately, which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. +Composed bringup (based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html)) is optional for users. It can be used to compose all Nav2 nodes in a single process instead of launching these nodes separately, which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. Manually composed bring up is used by default, but can be disabled by using the launch argument `use_composition:=False`. * Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175. * Currently, manual composition is used in this package. Dynamic composition is more flexible than manual composition, but is not currently applied in nav2 due to various issues, you could find more details here: https://github.com/ros-planning/navigation2/issues/2147. -### Pre-requisites: -* [Install ROS 2](https://index.ros.org/doc/ros2/Installation/Dashing/) -* Install Nav2 - - ```sudo apt install ros--navigation2``` - -* Install Nav2 Bringup - - ```sudo apt install ros--nav2-bringup``` - -* Install your robot specific package (ex:[Turtlebot 3](http://emanual.robotis.com/docs/en/platform/turtlebot3/ros2/)) - -## Launch Nav2 in *Simulation* with Gazebo -### Pre-requisites: - -* [Install Gazebo](http://gazebosim.org/tutorials?tut=install_ubuntu&cat=install) -* gazebo_ros_pkgs for ROS2 installed on the system - - ```sudo apt-get install ros--gazebo*``` -* A Gazebo world for simulating the robot ([Gazebo tutorials](http://gazebosim.org/tutorials?tut=quick_start)) -* A map of that world saved to a map.pgm and map.yaml ([ROS Navigation Tutorials](https://github.com/ros-planning/navigation2/tree/main/doc/use_cases)) - -### Terminal 1: Launch Gazebo - -Example: See [turtlebot3_gazebo models](https://github.com/ROBOTIS-GIT/turtlebot3_simulations/tree/ros2/turtlebot3_gazebo/models) for details - -```bash -export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH: -gazebo --verbose -s libgazebo_ros_init.so -``` - -### Terminal 2: Launch your robot specific transforms - -Example: See [turtlebot3_gazebo](https://github.com/ROBOTIS-GIT/turtlebot3_simulations/tree/ros2/turtlebot3_gazebo) for details - -```bash -source /opt/ros/dashing/setup.bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py use_sim_time:=True -``` - -### Terminal 3: Launch Nav2 - -Normal Bringup - -```bash -source /opt/ros/dashing/setup.bash -ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True autostart:=True use_composition:=False\ -map:= -``` - -Manually Composed Bringup - -```bash -source /opt/ros/dashing/setup.bash -ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True autostart:=True use_composition:=True\ -map:= -``` -* `use_composition` is True by default - -### Terminal 4: Run RViz with Nav2 config file - -```bash -source /opt/ros/dashing/setup.bash -ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/launch/nav2_default_view.rviz -``` - -In RViz: -* You should see the map -* Localize the robot using “2D Pose Estimate” button. -* Make sure all transforms from odom are present. (odom->base_link->base_scan) -* Send the robot a goal using "Nav2 Goal” button. -Note: this uses a ROS2 Action to send the goal, and a pop-up window will appear on your screen with a 'cancel' button if you wish to cancel - -To view the robot model in RViz: -* Add "RobotModel", set "Description Source" with "File", set "Description File" with the name of the urdf file for your robot (example: turtlebot3_burger.urdf)" - -### Advanced: single-terminal launch - -A convenience file is provided to launch Gazebo, RVIZ and Nav2 using a single command: - -```bash -ros2 launch nav2_bringup tb3_simulation_launch.py -``` - -Where `` can used to replace any of the default options, for example: - -``` -use_composition:= -world:= -map:= -rviz_config_file:= -simulator:= -bt_xml_file:= -``` - - -Before running the command make sure you are sourcing the `ROS2` workspace, setting the path to the Gazebo model and defining the TB3 robot model to use. - -```bash -source -export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH: -export TURTLEBOT3_MODEL=waffle -``` - -Also, a file for launching **two** robots with **independent** navigation stacks is provided: - -```bash -ros2 launch nav2_bringup multi_tb3_simulation_launch.py -``` - - -## Launch Nav2 on a *Robot* - -### Pre-requisites: -* Run SLAM with Navigation 2 or tele-op to drive the robot and generate a map of an area for testing first. The directions below assume this has already been done or there is already a map of the area. - -* Learn more about how to use Navigation 2 with SLAM to create maps; - - - [Navigation 2 with SLAM](https://github.com/ros-planning/navigation2/blob/main/doc/use_cases/navigation_with_slam.md) - -* _Please note that currently, nav2_bringup works if you provide a map file. However, providing a map is not required to use Nav2. Nav2 can be configured to use the costmaps to navigate in an area without using a map file_ - -* Publish all the transforms from your robot from base_link to base_scan - - -### Terminal 1 : Launch Nav2 using your map.yaml - -```bash -source /opt/ros/dashing/setup.bash -ros2 launch nav2_bringup bringup_launch.py map:= map_type:=occupancy -``` - -### Terminal 2 : Launch RVIZ - -```bash -source /opt/ros/dashing/setup.bash -ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/launch/nav2_default_view.rviz -``` - -In RVIZ: -* Make sure all transforms from odom are present. (odom->base_link->base_scan) -* Localize the robot using “2D Pose Estimate” button. -* Send the robot a goal pose using “2D Nav Goal” button. +To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. Note: -* nav2_gazebo_spawner pkg inside nav2_bringup directory is deleted. -* use of nav2_gazebo_spawner to spawn the robot in gazebo is not recommended any more. Instead use spawn_entity.py of gazebo_ros to spawn the robot. * gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly. -* spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. Instead it should be done as remapping argument /tf:=tf /tf_static:=tf_static under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf's of the different robot. \ No newline at end of file +* spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. Instead it should be done as remapping argument /tf:=tf /tf_static:=tf_static under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf's of the different robot. diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index 112555570d..8dd51a6c6e 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -1,136 +1,9 @@ # BT Navigator -The BT Navigator (Behavior Tree Navigator) module implements the [NavigateToPose task interface](../nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp). It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors. +The BT Navigator (Behavior Tree Navigator) module implements the NavigateToPose and NavigateThroughPoses task interfaces. It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors. -NOTE: This readme may be outdated, please reference navigation.ros.org for the most current information. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. ## Overview -The BT Navigator receives a goal pose and navigates the robot to the specified destination. To do so, the module reads an XML description of the Behavior Tree from a file, as specified by a Node parameter, and passes that to a generic [BehaviorTreeEngine class](../nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp) which uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) to dynamically create and execute the BT. - -## Specifying an input XML file - -The BtNavigator node has a parameter, *bt_xml_filename*, that can be specified using a ROS2 parameters YAML file, like this: - -``` -BtNavigator: - ros__parameters: - bt_xml_filename: -``` - -Using the XML filename as a parameter makes it easy to change or extend the logic used for navigation. Once can simply update the XML description for the BT and the BtNavigator task server will use the new description. - -## Behavior Tree nodes - -A Behavior Tree consists of control flow nodes, such as fallback, sequence, parallel, and decorator, as well as two execution nodes: condition and action nodes. Execution nodes are the leaf nodes of the tree. When a leaf node is ticked, the node does some work and it returns either SUCCESS, FAILURE or RUNNING. The current Nav2 software implements a few custom nodes, including Conditions and Actions. The user can also define and register additional node types that can then be used in BTs and the corresponding XML descriptions. - -## Navigation Behavior Trees - -The BT Navigator package has four sample XML-based descriptions of BTs. -These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml), [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml) and -[follow_point.xml](behavior_trees/follow_point.xml). -The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs. - -### Navigate with Replanning (time-based) - -[navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) implements basic navigation by continuously computing and updating the path at a rate of 1Hz. The default controller, the nav2_dwb_controller, implements path following at a rate of 10Hz. - -![Navigation with time based replanning](./doc/navigate_w_replanning_time.png) - -### Navigate with Replanning (distace-based) - -[navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml) implements basic navigation by continuously computing and updating the path after every 1 meter distance traveled by the robot. - -![Navigation with distance based replanning](./doc/navigate_w_replanning_distance.png) - -#### Navigate with replanning is composed of the following custom decorator, condition, control and action nodes: - -#### Control Nodes -* PipelineSequence: This is a variant of a Sequence Node. When this node is ticked, -it will tick the first child till it succeeds. Then it will tick the first two -children till the second one succeeds. Then it will tick the first three till the -third succeeds and so on, till there are no more children. This will return RUNNING, -till the last child succeeds, at which time it also returns SUCCESS. If any child -returns FAILURE, all nodes are halted and this node returns FAILURE. -* RoundRobin: This is a custom control node introduced to the Behavior Tree. When this node is ticked, it will tick the first child until it returns SUCCESS or FAILURE. If the child returns either SUCCESS or FAILURE, it will tick its next child. Once the node reaches the last child, it will restart ticking from the first child. The main difference between the RoundRobin node versus the Sequence node is that when a child returns FAILURE the RoundRobin node will tick the next child but in the Sequence node, it will return FAILURE. - -* Recovery: This is a control flow type node with two children. It returns success if and only if the first child returns success. The second child will be executed only if the first child returns failure. The second child is responsible for recovery actions such as re-initializing system or other recovery behaviors. If the recovery behaviors are succeeded, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning failure. The figure below depicts a simple recovery node. - -

- -

-
- -#### Decorator Nodes -* RateController: A custom control flow node, which throttles down the tick rate. This custom node has only one child and its tick rate is defined with a pre-defined frequency that the user can set. This node returns RUNNING when it is not ticking its child. Currently, in the navigation, the `RateController` is used to tick the `ComputePathToPose` and `GoalReached` node at 1 Hz. - -* DistanceController: A custom control flow node, which controls the tick rate based on the distance traveled. This custom node has only one child. The user can set the distance after which the planner should replan a new path. This node returns RUNNING when it is not ticking its child. Currently, in navigation, the `DistanceController` is used to tick the `ComputePathToPose` and `GoalReached` node after every 0.5 meters. - -* SpeedController: A custom control flow node, which controls the tick rate based on the current speed. This decorator offers the most flexibility as the user can set the minimum/maximum tick rate which is adjusted according to the current speed. - -* GoalUpdater: A custom control node, which updates the goal pose. It subscribes to a topic in which it can receive an updated goal pose to use instead of the one commanded in action. It is useful for dynamic object following tasks. - -#### Condition Nodes -* GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked. - -#### Action Nodes -* ComputePathToPose: When this node is ticked, the goal will be placed on the blackboard which will be shared to the Behavior tree. The bt action node would then utilizes the action server to send a request to the global planner to recompute the global path. Once the global path is recomputed, the result will be sent back via action server and then the updated path will be placed on the blackboard. The `planner` parameter will tell the planning server which of the loaded planning plugins to utilize, in case of desiring different parameters, planners, or behaviors. The name of the planner should correspond with the high level task it accomplishes and align with the `planner_ids` name given to it in the planner server. If no planner name is provided, it will use the only planner in the planner server when only one is available. - -The graphical version of this Behavior Tree: - -

- -

-
- -The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal to compute a control effort for. `controller_id` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowPathSlow` `FollowPathExactly`, etc. - - -* TruncatePath: A custom control node, which modifies a path making it shorter. It removes parts of the path closer than a distance to the goal pose. The resulting last pose of the path orientates the robot to the original goal pose. - -### Navigate with replanning and simple recovery actions - -With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin`, `wait`, and `backup`. - -This behavior tree implements multi-scope recovery where both the global planner and the controller have their own recovery actions on failure in addition to the main recovery subtree that gets triggered if the planner or controller continues to fail. The planner/controller specific recovery subtrees contain only a simple costmap clearing action in this BT. The main recovery subtree triggers recovery actions in a round robin fashion where the navigation subtree is retried after trying one recovery action. If the navigation subtree fails even after a retry, the `RoundRobin` node triggers the next recovery action in the sequence and the navigation subtree is retried. This cycle continues until: -- The navigation subtree succeeds -- All recovery actions fail -- Specified number of retries is exceeded - -All recovery actions are preemptable and are halted when a new navigation goal arrives. - -A graphical version of this simple recoverable Behavior Tree is depicted in the figure below. - -

- -

-
- - -This tree is currently our default tree in the stack and the xml file is located here: [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml). - - -#### Halting recoveries on navigation goal (Preemption reference design) -In general, the recovery behaviours and any other long running process should be stopped when the navigation goal is issued (e.g. preemption). In the default tree in the stack, this behaviour is accomplished using a condition node checking the global navigation goal and a reactive fallback controller node: - -

- -

- -This way, the recovery actions can be interrupted if a new goal is sent to the bt_navigator. Adding other condition nodes to this structure, it is possible to halt the recoveries in other cases (e.g, giving a time limit for their execution). This is the recommended design pattern for preempting a node or tree branch under specific conditions such as a new navigation request. Please notice that the order of definition of the nodes in the xml file can alter the behaviour of this structure, and that all conditions should be placed before the recovery behaviour node or branch. - - -#### Multi-Scope Recoveries -Scope-based failure handling: Utilizing Behavior Trees with a recovery node allows one to handle failures at multiple scopes. With this capability, any action in a large system can be constructed with specific recovery actions suitable for that action. Thus, failures in these actions can be handled locally within the scope. With such design, a system can be recovered at multiple levels based on the nature of the failure. Higher level recovery actions could be recovery actions such as re-initializing the system, re-calibrating the robot, bringing the system to a good known state, etc. - -### Navigate following a dynamic point to a certain distance -This tree is an example of how behavior trees can be used to make the robot do more than navigate the current position to the desired pose. In this case, the robot will follow a point that changes dynamically during the execution of the action. The robot will stop its advance and will be oriented towards the target position when it reaches a distance to the target established in the tree. The UpdateGoal decorator node will be used to update the target position. The TruncatePath node will modify the generated path by removing the end part of this path, to maintain a distance from the target, and changes the orientation of the last position. This tree never returns that the action has finished successfully, but must be canceled when you want to stop following the target position. - -![Navigate following a dynamic point to a certain distance](./doc/follow_point.png) - -This tree currently is not our default tree in the stack. The xml file is located here: [follow_point.xml](behavior_trees/follow_point.xml). - -## Legend -Legend for the behavior tree diagrams: - -![Legend](./doc/legend.png) +The BT Navigator receives a goal pose and navigates the robot to the specified destination(s). To do so, the module reads an XML description of the Behavior Tree from a file, as specified by a Node parameter, and passes that to a generic [BehaviorTreeEngine class](../nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp) which uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) to dynamically create and execute the BT. The BT XML can also be specified on a per-task basis so that your robot may have many different types of navigation or autonomy behaviors on a per-task basis. diff --git a/nav2_controller/README.md b/nav2_controller/README.md index b0f48490a6..3a77144f16 100644 --- a/nav2_controller/README.md +++ b/nav2_controller/README.md @@ -1,8 +1,9 @@ # Nav2 Controller -The Nav2 Controller is an [Execution Module](../doc/requirements/requirements.md) that implements the `nav2_msgs::action::FollowPath` action server. +The Nav2 Controller is a Task Server in Nav2 that implements the `nav2_msgs::action::FollowPath` action server. -An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. +An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with multiple plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. It also contains progress checkers and goal checker plugins to abstract out that logic from specific controller implementations. +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available controller plugins. -Currently available controller plugins are: DWB, and [TEB (dashing release)](https://github.com/rst-tu-dortmund/teb_local_planner/tree/dashing-devel). \ No newline at end of file +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html). diff --git a/nav2_core/README.md b/nav2_core/README.md index 062fda5a4d..f4bf7f5c18 100644 --- a/nav2_core/README.md +++ b/nav2_core/README.md @@ -2,6 +2,11 @@ This package hosts the abstract interface (virtual base classes) for plugins to be used with the following: - global planner (e.g., `nav2_navfn_planner`) -- controller (i.e, path execution controller, e.g `nav2_dwb_controller`) -- goal checker -- recovery behaviors +- controller (e.g., path execution controller, e.g `nav2_dwb_controller`) +- goal checker (e.g. `simple_goal_checker`) +- recovery behaviors (e.g. `backup`) +- progress checker (e.g. `simple_progress_checker`) +- waypoint task executor (e.g. `take_pictures`) +- exceptions in planning and control + +The purposes of these plugin interfaces are to create a separation of concern from the system software engineers and the researcher / algorithm designers. Each plugin type is hosted in a "task server" (e.g. planner, recovery, control servers) which handles requests and multiple algorithm plugin instances. The plugins are used to compute a value back to the server without having to worry about ROS 2 actions, topics, or other software utilities. A plugin designer can simply use the tools provided in the API to do their work, or create new ones if they like internally to gain additional information or capabilities. diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index 238eecce2d..b2eb9a2b94 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -1,60 +1,12 @@ # Nav2 Costmap_2d -The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors A plugin interface allows for the layers to be combined into the -costmap and finally inflated via a user specified inflation radius. The nav2 version of the costmap_2d package is mostly a direct -ROS2 port of the ROS1 navigation stack version, with minimal noteable changes necessary due to support in ROS2. +The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal noteable changes necessary due to support in ROS2. -## Overview of Changes from ROS1 Navigation Costmap_2d -- Removal of legacy parameter style ("Loading from pre-hydro parameter style") -- Intermediate replacement of dynamic reconfigure (not ported to ROS2). This discussion started here with costmap_2d but is a more -widespread discussion throughout the navigation stack (see issue https://github.com/ros-planning/navigation2/issues/177) and -general ROS2 community. A proposal temporary replacement has been submitted as a PR here: https://github.com/ros-planning/navigation2/pull/196 +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://navigation.ros.org/tutorials/index.html) and [first-time setup guides](https://navigation.ros.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins. -## How to configure using Voxel Layer plugin: -By default, the navigation stack uses the _Obstacle Layer_ to avoid obstacles in 2D. The _Voxel Layer_ allows for detecting obstacles in 3D using Pointcloud2 data. It requires Pointcloud2 data being published on some topic. For simulation, a Gazebo model of the robot with depth camera enabled will publish a pointcloud topic. +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. -The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugins``` and its ```plugin``` type should be correctly specified in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used. - -Inside each costmap layer (voxel, obstacle, etc) define `observation_sources` param. Here you can define multiple sources to be used with the layer. The param configuration example below shows the way you can configure costmaps to use voxel layer. - -The `voxel_layer` param has `observation_source: pointcloud` in it's scope, and the param `pointcloud` has `topic`, and `data_type` inside it's scope. By default, the data_type is `LaserScan`, thus you need to specify `PointCloud2` if you are using PointCould2 data from a depth sensor. - -Example param configuration snippet for enabling voxel layer in local costmap is shown below (not all params are shown): -``` -local_costmap: - local_costmap: - ros__parameters: - plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - voxel_layer: - plugin: nav2_costmap_2d::VoxelLayer - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.2 - z_voxels: 10 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: pointcloud - pointcloud: - topic: /intel_realsense_r200_depth/points - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "PointCloud2" -``` -Please note that not all params needed to run the navigation stack are shown here. This example only shows how you can add different costmap layers, with multiple input sources of different types. - -### To visualize the voxels in RVIZ: +## To visualize the voxels in RVIZ: - Make sure `publish_voxel_map` in `voxel_layer` param's scope is set to `True`. - Open a new terminal and run: ```ros2 run nav2_costmap_2d nav2_costmap_2d_markers voxel_grid:=/local_costmap/voxel_grid visualization_marker:=/my_marker``` @@ -63,40 +15,13 @@ Please note that not all params needed to run the navigation stack are shown her - Then add `my_marker` to RVIZ using the GUI. -#### Errata: +### Errata: - To see the markers in 3D, you will need to change the _view_ in RVIZ to a 3 dimensional view (e.g. orbit) from the RVIZ GUI. - Currently due to some bug in rviz, you need to set the `fixed_frame` in the rviz display, to `odom` frame. - Using pointcloud data from a saved bag file while using gazebo simulation can be troublesome due to the clock time skipping to an earlier time. -## How to use multiple sensor sources: -Multiple sources can be added into a costmap layer (e.g., obstacle layer), by listing them under the 'observation_sources' for that layer. -For example, to add laser scan and pointcloud as two different sources of inputs to the obstacle layer, you can define them in the params file as shown below for the local costmap: -``` -local_costmap: - local_costmap: - ros__parameters: - plugins: ["obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan pointcloud - scan: - topic: /scan - data_type: "LaserScan" - pointcloud: - topic: /intel_realsense_r200_depth/points - data_type: "PointCloud2" -``` -In order to add multiple sources to the global costmap, follow the same procedure shown in the example above, but now adding the sources and their specific params under the `global_costmap` scope. - ## Costmap Filters ### Overview Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on Nav2 website: https://navigation.ros.org. - -## Future Plans -- Conceptually, the costmap_2d model acts as a world model of what is known from the map, sensor, robot pose, etc. We'd like -to broaden this world model concept and use costmap's layer concept as motivation for providing a service-style interface to -potential clients needing information about the world (see issue https://github.com/ros-planning/navigation2/issues/18) - diff --git a/nav2_dwb_controller/README.md b/nav2_dwb_controller/README.md index 976cdc646f..b440f29a82 100644 --- a/nav2_dwb_controller/README.md +++ b/nav2_dwb_controller/README.md @@ -1,37 +1,16 @@ -# Local Planner +# DWB Controller -## Local Planner under ROS 1 +The DWB controller is the successor to the base local planner and DWA controllers in ROS 1. It was created in ROS 1 by David Lu!! at Locus Robotics as part of the `robot_navigation` project. It was then ported to ROS 2 for use in Nav2 as its critic-based controller algorithm. -Under ROS 1, the navigation stack provides a `BaseLocalPlanner` interface -used by `MoveBase`. The `BaseLocalPlanner` component is expected to take a path and current position and produce a command velocity. The most commonly used implementation of the local planner uses the DWA algorithm, however, the Trajectory Rollout algorithm is also available in the ROS 1 navigation stack. +DWB improves on DWA in a few major ways: -The navigation repository provides two implementations of the DWA algorithm: `base_local_planner` and `dwa_local_planner`. In addition, there is another DWA based planner in the [Robot Navigation repository](https://github.com/locusrobotics/robot_navigation) called DWB. The `dwa_local_planner` was meant to replace `base_local_planner` and provides a better DWA algorithm, but failed to -provide the Trajectory Rollout algorithm, so was unable to completely replace -`base_local_planner`. +- It implements plugin-based critics to allow users to specify new critic functions to use in the system. They can be dynamically reconfigured, reweighted, and tuned to gain very particular behavior in your robot system. +- It implements plugin-based trajectory generation techniques, so that users can generate trajectories any number of ways and for any number of types of vehicles +- Includes a number of plugin implementations for common use -DWB was meant to replace both the planners in the navigation repository and provides -an implementation of both DWA and Trajectory Rollout. +It is possible to tune DWB to gain both DWA and base local planner behaviors, as well as expansions using new plugins for totally use-case specific behaviors. The current trajectory generator plugins work for omnidirectional and differential drive robots, though an ackermann generator would be trivial to add. The current critic plugins work for both circular and non-circular robots and include many of the cost functions needed to build a path tracking system with various attributes. -![Local Planner Structure](./images/LocalPlanner.svg "Local planner structure under ROS 1") - -## Migrating to ROS 2 - -Rather than continue with 3 overlapping implementations of DWA, the DWB planner -was chosen to use as the default controller plugin. - -The advantages of DWB were: -* it was designed with backwards compatibility with the other controllers -* it was designed with clear extension points and modularity - -## Changes to DWB - -For the most part, the DWB codebase is unchanged, other than what was required for ROS2 and the `nav2_core` interface for Controllers. - -![ROS 1 DWB Structure](./images/DWB_Structure_Simplified.svg "ROS 1 DWB Structure") - -## New local planner interface - -See `nav2_core` `Controller` interface, which defines an interface for the controller plugins. These plugins are loaded into the `nav2_controller_server` to compute control commands from requests to the ROS2 action server. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-dwb-controller.html) for additional parameter descriptions. ## DWB Plugins @@ -52,15 +31,6 @@ loaded at a time. algorithm used in base_local_planner in ROS 1. * **LimitedAccelGenerator** - This is similar to DWA used in ROS 1. -### Goal Checker Plugins - -These plugins check whether we have reached the goal or not. Again, only one can -be loaded at a time. - -* **SimpleGoalChecker** - This checks whether the robot has reached the goal pose -* **StoppedGoalChecker** - This checks whether the robot has reached the goal pose - and come to a stop. - ### Critic Plugins These plugins score the trajectories generated by the trajectory generator. diff --git a/nav2_lifecycle_manager/README.md b/nav2_lifecycle_manager/README.md index 83b4a61c1b..9a0905006c 100644 --- a/nav2_lifecycle_manager/README.md +++ b/nav2_lifecycle_manager/README.md @@ -1,11 +1,12 @@ ### Background on lifecycle enabled nodes Using ROS2’s managed/lifecycle nodes feature allows the system startup to ensure that all required nodes have been instantiated correctly before they begin their execution. Using lifecycle nodes also allows nodes to be restarted or replaced on-line. More details about managed nodes can be found on [ROS2 Design website](https://design.ros2.org/articles/node_lifecycle.html). Several nodes in Nav2, such as map_server, planner_server, and controller_server, are lifecycle enabled. These nodes provide the required overrides of the lifecycle functions: ```on_configure()```, ```on_activate()```, ```on_deactivate()```, ```on_cleanup()```, ```on_shutdown()```, and ```on_error()```. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-lifecycle.html) for additional parameter descriptions. ### nav2_lifecycle_manager -Nav2's lifecycle manager is used to change the states of the lifecycle nodes in order to achieve a controlled _startup_, _shutdown_, _reset_, _pause_, or _resume_ of the navigation stack. The lifecycle manager presents a ```lifecycle_manager/manage_nodes``` service, from which clients can invoke the startup, shutdown, reset, pause, or resume functions. Based on this service request, the lifecycle manager calls the necessary lifecycle services in the lifecycle managed nodes. Currently, the RVIZ panel uses this ```lifecycle_manager/manage_nodes``` service when user presses the buttons on the RVIZ panel (e.g.,startup, reset, shutdown, etc.). +Nav2's lifecycle manager is used to change the states of the lifecycle nodes in order to achieve a controlled _startup_, _shutdown_, _reset_, _pause_, or _resume_ of the navigation stack. The lifecycle manager presents a ```lifecycle_manager/manage_nodes``` service, from which clients can invoke the startup, shutdown, reset, pause, or resume functions. Based on this service request, the lifecycle manager calls the necessary lifecycle services in the lifecycle managed nodes. Currently, the RVIZ panel uses this ```lifecycle_manager/manage_nodes``` service when user presses the buttons on the RVIZ panel (e.g.,startup, reset, shutdown, etc.), but it is meant to be called on bringup through a production system application. -In order to start the navigation stack and be able to navigate, the necessary nodes must be configured and activated. Thus, for example when _startup_ is requested from the lifecycle manager's manage_nodes service, the lifecycle managers calls _configure()_ and _activate()_ on the lifecycle enabled nodes in the node list. +In order to start the navigation stack and be able to navigate, the necessary nodes must be configured and activated. Thus, for example when _startup_ is requested from the lifecycle manager's manage_nodes service, the lifecycle managers calls _configure()_ and _activate()_ on the lifecycle enabled nodes in the node list. These are all transitioned in ordered groups for bringup transitions, and reverse ordered groups for shutdown transitions. The lifecycle manager has a default nodes list for all the nodes that it manages. This list can be changed using the lifecycle manager’s _“node_names”_ parameter. diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index a5f87748e1..64ef980be9 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -1,15 +1,9 @@ # Map Server The `Map Server` provides maps to the rest of the Nav2 system using both topic and -service interfaces. +service interfaces. Map server will expose maps on the node bringup, but can also change maps using a `load_map` service during run-time, as well as save maps using a `save_map` server. -## Changes from ROS1 Navigation Map Server - -While the nav2 map server provides the same general function as the nav1 map server, the new -code has some changes to accomodate ROS2 as well as some architectural improvements. - -In addition, there is now two new "load_map" and "save_map" services which can be used to -dynamically load and save a map. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-map-server.html) for additional parameter descriptions. ### Architecture diff --git a/nav2_msgs/README.md b/nav2_msgs/README.md index b71a9c899f..868c049f0a 100644 --- a/nav2_msgs/README.md +++ b/nav2_msgs/README.md @@ -1,5 +1,3 @@ # nav2_msgs -The `nav2_msgs` package is a set of messages, services, and actions for the `Nav2` system. `Nav2` still makes use of `nav_msgs` from ROS (1) Navigation. - -See the ROS 1 to ROS 2 [Migration Guide](https://index.ros.org/doc/ros2/Migration-Guide/#messages-and-services) for details about use of the new message and service types. +The `nav2_msgs` package is a set of messages, services, and actions for the `Nav2` system. `Nav2` also makes use of the standard `nav_msgs`, where appropriate. diff --git a/nav2_navfn_planner/README.md b/nav2_navfn_planner/README.md index 90bb5467b3..ad980d1765 100644 --- a/nav2_navfn_planner/README.md +++ b/nav2_navfn_planner/README.md @@ -1,19 +1,7 @@ # Navfn Planner -The NavfnPlanner is a plugin for the Nav2 Planner server. +The NavfnPlanner is a global planner plugin for the Nav2 Planner server. It implements the Navigation Function planner with either A\* or Dij. expansions. It is largely equivalent to its counterpart in ROS 1 Navigation. The Navfn planner assumes a circular robot (or a robot that can be approximated as circular for the purposes of global path planning) and operates on a weighted costmap. -It provides the equivalent functionality to a [GlobalPlanner](http://wiki.ros.org/nav_core#BaseGlobalPlanner) in ROS1 [MoveBase](http://wiki.ros.org/move_base). +The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner. -## Status -Currently, NavfnPlanner's core algorithm is a direct port from the ROS1 MoveBase [Navfn](http://wiki.ros.org/navfn) planner. The Navfn planning algorithm is based on the [Global Dynamic Window Approach](https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf). - -## Characteristics - -In Dijkstra mode (`use_astar = false`) Dijkstra's search algorithm is guaranteed to find the shortest path under any condition. -In A* mode (`use_astar = true`) A*'s search algorithm is not guaranteed to find the shortest path, however it uses a heuristic to expand the potential field towards the goal. - -The Navfn planner assumes a circular robot and operates on a costmap. - -## Next Steps -- Implement additional planners based on optimal control, potential field or other graph search algorithms that require transformation of the world model to other representations (topological, tree map, etc.) to confirm sufficient generalization. [Issue #225](http://github.com/ros-planning/navigation2/issues/225) -- Implement planners for non-holonomic robots. [Issue #225](http://github.com/ros-planning/navigation2/issues/225) +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions. diff --git a/nav2_planner/README.md b/nav2_planner/README.md index 0d4447b103..3a17c825d4 100644 --- a/nav2_planner/README.md +++ b/nav2_planner/README.md @@ -1,5 +1,9 @@ # Nav2 Planner -The Nav2 planner is a [planning module](../doc/requirements/requirements.md) that implements the `nav2_behavior_tree::ComputePathToPose` interface. +The Nav2 planner is a Task Server in Nav2 that implements the `nav2_behavior_tree::ComputePathToPose` interface. -A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins like NavFn to do the path generation in different user-defined situations. +A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins to do the path generation in different user-defined situations. + +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html). diff --git a/nav2_recoveries/README.md b/nav2_recoveries/README.md index e5f93a0f99..beecd7cdac 100644 --- a/nav2_recoveries/README.md +++ b/nav2_recoveries/README.md @@ -1,120 +1,15 @@ -**Warning**: As with the rest of `nav2`, this package is still in development and only works with Turtlebot 3 at the moment. Currently collision avoidance has not been integrated. The user is advised to not use this feature on a physical robot for safety reasons. As of now, this feature should only be used in simulations. - ---- - # Recoveries -The `nav2_recoveries` package implements, as the name suggests, a module for executing simple controlled robot movements such as rotating on its own axis or moving linearly. +The `nav2_recoveries` package implements a task server for executing simple controlled robot movements such as rotating on its own axis, assisted teleop, or moving linearly. The package defines: -- A `Recovery` template which is used as a base class to implement specific recovery. -- The `BackUp`, `Spin` and `Stop` recoveries. - -## Overview - -*Recovery* define simple predictable movements that components can leverage for defining more complex behavior. For example, `nav2` uses recoveries for executing recovery behaviors, such as the ones defined on the [BtNavigator](../nav2_bt_navigator/README.md##Recovery-Behavior-Trees). - -Currently the package provides the following recoveries: -- **Spin** performs an in-place rotation by a given angle. -- **Back Up** performs an linear translation by a given distance. -- **Stop** brings the robot to a stationary state. - -## Implementation - -The module is implemented as a single node containing multiple recoveries and follows the `nav2` [task hierarchy](../nav2_tasks/README.md#Overview). Each recovery is defined as a `nav2_task` with corresponding *command* and *result* message definitions. - -The `Recovery` base class manages the task server, provides a robot interface and calls the recovery's update functions. - -To gain insight into the package lets go over how to implement and execute a new recoveries. - -### Defining a recovery - -In this section we'll go over how to define a new recovery and implement the corresponding recovery. - -The first step is to provide the [task definition](../nav2_tasks/README.md) inside the `nav2_tasks` package. For example, lets define a `SomeRecovery` task interface, i.e. the types of messages to use for the command and result, as well as the client and server. - -```cpp -namespace nav2_tasks -{ - -using SomeRecoveryCommand = geometry_msgs::msg::Point; -using SomeRecoveryResult = std_msgs::msg::Empty; - -using SomeRecoveryTaskClient = TaskClient; -using SomeRecoveryTaskServer = TaskServer; - -template<> -inline const char * getTaskName() -{ - return "SomeRecoveryTask"; -} - -} // namespace nav2_tasks -``` - -For this example we arbitrarily pick `geometry_msgs::msg::Point` and `std_msgs::msg::Empty` as message types for command and result. - -Next we define the class for our new recovery. This class should derive from `Recovery` and use the command and result messages defined on the corresponding task. - -```cpp -class SomeRecovery : public Recovery -``` - -On the implementation of `SomeRecovery` all we do is override `onRun` and `onCycleUpdate`. - -```cpp -using nav2_tasks - -TaskStatus SomeRecovery::onRun(const SomeRecoveryCommand::SharedPtr command) -{ - /* onRun code */ -} - -TaskStatus SomeRecovery::onCycleUpdate(SomeRecoveryResult & result) -{ - /* onCycleUpdate code */ -} -``` - -The `onRun` method is the entry point for the recovery and here we should: -- Catch the command. -- Perform checks before the main execution loop. -- Possibly do some initialization. -- Return a `nav2_tasks::TaskStatus` given the initial checks. - -The `onCycleUpdate` method is called periodically until it returns `FAILED` or `SUCCEEDED`, here we should: -- Set the robot in motion. -- Perform some unit of work. -- Check if the robot state, determine if work completed -- Return a `nav2_tasks::TaskStatus`. - -### Defining the recovery's client - -Recoveries use the `nav2_tasks` interface, so we need to define the task client: - -```cpp -nav2_tasks::TaskClient some_recovery_task_client; -``` - -To send requests we create the command and sent it over the client: - -```cpp -SomeRecoveryCommand::SharedPtr command; -// Fill command -some_recovery_task_client.sendCommand(command) -``` - -### (optional) Define the Behavior Tree action node +- A `Recovery` template which is used as a base class to implement specific recovery timed action server - but not required. +- The `BackUp`, `Spin` and `Wait` recoveries. -For using recoveries within a behavior tree such as [bt_navigator](../nav2_bt_navigator/README.md##Navigation-Behavior-Trees), then a corresponding *action node* needs to be defined. Checkout `nav2_tasks` for examples on how to implement one. +The only required class a recovery must derive from is the `nav2_core/recovery.hpp` class, which implements the pluginlib interface the recovery server will use to dynamically load your behavior. The `nav2_recoveries/recovery.hpp` derives from this class and implements a generic action server for a timed recovery behavior (e.g. calls an implmentation function on a regular time interval to compute a value) but **this is not required** if it is not helpful. A behavior does not even need to be an action if you do not wish, it may be a service or other interface. However, most motion and behavior primitives are probably long-running and make sense to be modeled as actions, so the provided `recovery.hpp` helps in managing the complexity to simplify new behavior development, described more below. -## Plans +The value of the centralized recovery server is to **share resources** amongst several behaviors that would otherwise be independent nodes. Subscriptions to TF, costmaps, and more can be quite heavy and add non-trivial compute costs to a robot system. By combining these independent behaviors into a single server, they may share these resources while retaining complete independence in execution and interface. -- Check for collision before executing a recovery. Issues [379](https://github.com/ros-planning/navigation2/issues/379) and [533](https://github.com/ros-planning/navigation2/issues/533). -- Remove the stop recovery, move the funcionality to the robot class. Issue [575](https://github.com/ros-planning/navigation2/issues/575) -- Consider moving `nav2_recoveries` altogether to the `nav2_robot` package. Issue [378](https://github.com/ros-planning/navigation2/issues/378). -- Depending on the feedback from the community we might want to develop this package to include a wide variety of recoveries (arcs) to support all kinds of task, navigation (lattice-based), docking, etc. -- Define smooth transitions between motions. Issue [411](https://github.com/ros-planning/navigation2/issues/411). -- Make the existing recoveries configurable for other robots. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-recovery-server.html) for additional parameter descriptions and a [tutorial about writing recovery behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_recovery_plugin.html). -Refer to Github for an up-to-date [list](https://github.com/ros-planning/navigation2/issues?q=is%3Aopen+is%3Aissue+label%3Anav2_recoveries). +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index c3dbbba57f..7ed735013c 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -14,6 +14,8 @@ This controller has been measured to run at well over 1 kHz on a modern intel pr

+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-regulated-pp.html) for additional parameter descriptions. + ## Pure Pursuit Basics The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 707e372294..1eb5b54f77 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -10,6 +10,8 @@ This was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41 ## API +See its [API Guide Page](https://navigation.ros.org/commander_api/index.html) for additional parameter descriptions. + The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type. | Robot Navigator Method | Description | diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 281f6c027f..d72af3400e 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -14,6 +14,8 @@ We have users reporting using this on: - Delivery robots - Industrial robots +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smac-planner.html) for additional parameter descriptions. + ## Introduction The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support **circular** differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support **cars, car-like, and ackermann vehicles** using the `SmacPlannerHybrid` plugin which implements a Hybrid-A\* planner. These plugins are also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. diff --git a/nav2_system_tests/README.md b/nav2_system_tests/README.md index c210085882..0456435942 100644 --- a/nav2_system_tests/README.md +++ b/nav2_system_tests/README.md @@ -1,36 +1,14 @@ # System Tests -The package provides tests for [components](#1.-Component-Testing), [subsystems](#2.-Subsystem-Testing) and full [system](#3.-System-Testing) integration. +The package provides tests for Component-Testing, Subsystem-Testing, and Full-System integration. Its main goal is to provide a location for smoke and integration tests of the navigation system to ensure that things are working properly on a high level. Unit and specific subsystem testing happens in the packages specific to those algorithms. -Unit tests are not included, these should be provided within each component or package. +Most tests in this package will spin up Gazebo instances of a robot in an environment to have the robot complete some task in the space while tracking a specific modules results. Some examples include -## 1. Component Testing -Test a component's ROS API (Pub/Sub/Service). +- System tests of a robot in a sandbox environment trying navigate to pose, navigate through poses, and waypoint following navigation types +- Random planning of thousands of paths in a generated environment to ensure default planners are working properly +- Testing the system can be brought up and down on the lifecycle transitions successfully multiple times +- Testing that the keepout and speed restricted zones work in a practical environment without going into keepout zones and slowing in speed restricted areas +- Testing recovery behaviors in a sandbox environment to ensure they trigger and complete collision checking properly +- Testing system failures are properly recorded and can be recovered from -- [Global Planning](src/planning/README.md) - -- Controller - -- [Localization](src/localization/README.md) - -- World Model - -- Costmaps - -## 2. Subsystem Testing -Test the integration of several components and subsystems. - -- Support modules (Mapping, Perception, Prediction, Localization - -- Navigation core (Navigator, Planner, Controller) - -- Support modules and navigation core - -- Command chain (Mission Planning, Mission Execution, Navigation System, Robot Interface) - -## 3. System Testing -Test the lifecycle startup and shutdown of nodes. - - [Updown Test](src/updown/README.md) - -Test the integration of all subsystems. - - [System Test](src/system/README.md) +This is primarily for use in Nav2 CI to establish a high degree of maintainer confidence when merging in large architectural changes to the Nav2 project. However, this is also useful to test installs of Nav2 locally or for additional information. diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index c888d0a6a4..edaf5e5299 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,6 +1,8 @@ # Nav2 Theta Star Planner The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta\* Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions. + ## Features - The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* - As it also considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases) diff --git a/nav2_util/README.md b/nav2_util/README.md index 98a53e3c07..844370983c 100644 --- a/nav2_util/README.md +++ b/nav2_util/README.md @@ -1,7 +1,12 @@ # Nav2 Util -The `nav2_util` package contains utilities abstracted from individual packages which may find use in other uses. Examples include the particle filter implementation from AMCL, motion models, ROS2 node utilities, and more. +The `nav2_util` package contains utilities abstracted from individual packages which may find use in other uses. Some examples of things you'll find here: -## ROS1 Comparison +- Geometry utilities for computing distances and values in paths +- A Nav2 specific lifecycle node wrapper for boilerplate code and useful common utilities like `declare_parameter_if_not_declared()` +- Simplified service clients +- Simplified action servers +- Transformation and robot pose helpers -This package does not have a direct counter-part in Navigation. This was created to abstract out sections of the code-base from their implementations should the base algorithms/utilities find use elsewhere. +The long-term aim is for these utilities to find more permanent homes in other packages (within and outside of Nav2) or migrate to the raw tools made available in ROS 2. + \ No newline at end of file diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 72e3bbee21..cd0a2366b9 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -2,6 +2,8 @@ The Nav2 waypoint follower is an example application of how to use the navigation action to complete some sort of orchestrated task. In this example, that task is to take a given set of waypoints and navigate to a set of positions in the order provided in the action request. The last waypoint in the waypoint array is the final position. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-waypoint-follower.html) for additional parameter descriptions. + The package exposes the `follow_waypoints` action server of type `nav2_msgs/FollowWaypoints`. It is given an array of waypoints to visit, gives feedback about the current index of waypoint it is processing, and returns a list of waypoints it was unable to complete. @@ -11,7 +13,7 @@ There is a parameterization `stop_on_failure` whether to stop processing the way ## An aside on autonomy / waypoint following -The ``nav2_waypoint_follower`` contains a waypoint following program with a plugin interface for specific task executors. +The ``nav2_waypoint_follower`` contains a waypoint following program with a plugin interface for specific **task executors**. This is useful if you need to go to a given location and complete a specific task like take a picture, pick up a box, or wait for user input. It is a nice demo application for how to use Nav2 in a sample application. @@ -20,7 +22,7 @@ There are 2 schools of thoughts for fleet managers / dispatchers. - Dumb robot; smart centralized dispatcher - Smart robot; dumb centralized dispatcher -In the first, the ``nav2_waypoint_follower`` is fully sufficient to create a production-grade on-robot solution. Since the autonomy system / dispatcher is taking into account things like the robot's pose, battery level, current task, and more when assigning tasks, the application on the robot just needs to worry about the task at hand and not the other complexities of the system complete the requested task. In this situation, you should think of a request to the waypoint follower as 1 unit of work (e.g. 1 pick in a warehouse, 1 security patrole loop, 1 aisle, etc) to do a task and then return to the dispatcher for the next task or request to recharge. In this school of thought, the waypoint following application is just one step above navigation and below the system autonomy application. +In the first, the ``nav2_waypoint_follower`` is weakly sufficient to create a production-grade on-robot solution. Since the autonomy system / dispatcher is taking into account things like the robot's pose, battery level, current task, and more when assigning tasks, the application on the robot just needs to worry about the task at hand and not the other complexities of the system complete the requested task. In this situation, you should think of a request to the waypoint follower as 1 unit of work (e.g. 1 pick in a warehouse, 1 security patrole loop, 1 aisle, etc) to do a task and then return to the dispatcher for the next task or request to recharge. In this school of thought, the waypoint following application is just one step above navigation and below the system autonomy application. In the second, the ``nav2_waypoint_follower`` is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the ``nav2_behavior_tree`` package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a ``nav2_bt_waypoint_follower`` (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy.