Adding a Controller from Nav2
As stated previously, the Controller is the magic that gets a robot to move, and governs a lot of the behaviour of how it navigates. It is also the most fiddly thing that will require tuning on a real robot to work reliably.
For a full list of Controllers available, visit the Nav 2 Controller Documentation.
The Controller
The Controller monitors the robot’s state (velocity, pose), the given path, and sensor measurements to compute velocity commands that lead to progress along the path.
Once again we have list of “plugins” to choose from, allowing us flexibility over what controller architecture to use. In fact, you can write your own controller (and planner)!
For this task we will stick with DWB planner, which is the general default choice.
Note
The name DWB is a inside joke made by David Lu. The algorithm used is called “Dynamic Window Approach” (DWA), however, as it was rewritten from ROS 1 to ROS 2, the author thought themselves hilarious to call it DWB (as in, “B” comes after “A”).
Note
The original DWA approach is based on the paper: Fox, D., Burgard, W. and Thrun, S. (1997). The Dynamic Window Approach to Collision Avoidance.
I told you that all these packages are based on research papers.
As mentioned, these Controllers can be tuned or altered to provide the behaviour you desire (e.g. how faithfully should it stick to the global path). It is not possible to exhaustively cover this tuning for every controller here, but most provide a tuning guide of sorts. For generic advice, please take a look at
Writing the Controller Config File
The format of the configuration is taken from the controller documentation. It consists of general controller parameters, but controller plugin specific parameters can also be passed. For DWB some of these parameters can be found here. An example config file for the DWB controller would look like the file below. Copy this example into a file called controller.yaml in the config directory.
1controller_server:
2 ros__parameters:
3 use_sim_time: True
4 controller_frequency: 20.0
5 min_x_velocity_threshold: 0.001
6 min_y_velocity_threshold: 0.5
7 min_theta_velocity_threshold: 0.001
8 failure_tolerance: 0.3
9 odom_topic: "odom"
10 progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older
11 goal_checker_plugin: "goal_checker"
12 controller_plugins: ["FollowPath"] # This is where we define the DWB controller plugin
13 progress_checker:
14 plugin: "nav2_controller::SimpleProgressChecker"
15 required_movement_radius: 0.5
16 movement_time_allowance: 10.0
17 goal_checker:
18 plugin: "nav2_controller::SimpleGoalChecker"
19 xy_goal_tolerance: 0.25
20 yaw_goal_tolerance: 0.25
21 stateful: True
22 FollowPath:
23 plugin: "dwb_core::DWBLocalPlanner"
24 debug_trajectory_details: True
25 min_vel_x: 0.0
26 min_vel_y: 0.0
27 max_vel_x: 0.26
28 max_vel_y: 0.0
29 max_vel_theta: 1.0
30 min_speed_xy: 0.0
31 max_speed_xy: 0.26
32 min_speed_theta: 0.0
33 acc_lim_x: 2.5
34 acc_lim_y: 0.0
35 acc_lim_theta: 3.2
36 decel_lim_x: -2.5
37 decel_lim_y: 0.0
38 decel_lim_theta: -3.2
39 vx_samples: 20
40 vy_samples: 5
41 vtheta_samples: 20
42 sim_time: 1.7
43 linear_granularity: 0.05
44 angular_granularity: 0.025
45 transform_tolerance: 0.2
46 xy_goal_tolerance: 0.25
47 trans_stopped_velocity: 0.25
48 short_circuit_trajectory_evaluation: True
49 stateful: True
50 critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
51 BaseObstacle.scale: 0.02
52 PathAlign.scale: 32.0
53 PathAlign.forward_point_distance: 0.1
54 GoalAlign.scale: 24.0
55 GoalAlign.forward_point_distance: 0.1
56 PathDist.scale: 32.0
57 GoalDist.scale: 24.0
58 RotateToGoal.scale: 32.0
59 RotateToGoal.slowing_factor: 5.0
60 RotateToGoal.lookahead_time: -1.0
The DWB planner has many options to tune the system. It is recommended that for your own robot, you would carefully read about all the different options and see which need to be altered.
Notice as well we needed to include a goal_checker and a progress_checker, these do pretty much what you would expect. By having these separate (rather than having them included in the controller plugin), it again allows for modularity. These can be tuned as well, for example, in the goal checker we set the tolerance of how close the robot must be to the goal (as it is nearly impossible for a robot to drive exactly to the point asked of it).
Writing the Local Costmap Config File
The Local costmap in comparison to the global costmap, only covers a small portion around the robot (which is quicker to compute), and is updated more regularly. In this case, we do not necessarily need the static map, just obstacles and other threats we wish to track. Because we are using layered costmaps, it is possible to add obstacle (or other types) of layers representing different sensors. We’ll just add the lidar for obstacles this time.
Again we include the inflation layer to convert obstacles into the configuration space of the robot. Edit the controller.yaml file where the DWB parameters were added earlier to match the example below.
1controller_server:
2 ros__parameters:
3 use_sim_time: True
4 controller_frequency: 20.0
5 min_x_velocity_threshold: 0.001
6 min_y_velocity_threshold: 0.5
7 min_theta_velocity_threshold: 0.001
8 failure_tolerance: 0.3
9 odom_topic: "odom"
10 progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older
11 goal_checker_plugin: "goal_checker"
12 controller_plugins: ["FollowPath"] # This is where we define the DWB controller plugin
13 progress_checker:
14 plugin: "nav2_controller::SimpleProgressChecker"
15 required_movement_radius: 0.5
16 movement_time_allowance: 10.0
17 goal_checker:
18 plugin: "nav2_controller::SimpleGoalChecker"
19 xy_goal_tolerance: 0.25
20 yaw_goal_tolerance: 0.25
21 stateful: True
22 FollowPath:
23 plugin: "dwb_core::DWBLocalPlanner"
24 debug_trajectory_details: True
25 min_vel_x: 0.0
26 min_vel_y: 0.0
27 max_vel_x: 0.26
28 max_vel_y: 0.0
29 max_vel_theta: 1.0
30 min_speed_xy: 0.0
31 max_speed_xy: 0.26
32 min_speed_theta: 0.0
33 acc_lim_x: 2.5
34 acc_lim_y: 0.0
35 acc_lim_theta: 3.2
36 decel_lim_x: -2.5
37 decel_lim_y: 0.0
38 decel_lim_theta: -3.2
39 vx_samples: 20
40 vy_samples: 5
41 vtheta_samples: 20
42 sim_time: 1.7
43 linear_granularity: 0.05
44 angular_granularity: 0.025
45 transform_tolerance: 0.2
46 xy_goal_tolerance: 0.25
47 trans_stopped_velocity: 0.25
48 short_circuit_trajectory_evaluation: True
49 stateful: True
50 critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
51 BaseObstacle.scale: 0.02
52 PathAlign.scale: 32.0
53 PathAlign.forward_point_distance: 0.1
54 GoalAlign.scale: 24.0
55 GoalAlign.forward_point_distance: 0.1
56 PathDist.scale: 32.0
57 GoalDist.scale: 24.0
58 RotateToGoal.scale: 32.0
59 RotateToGoal.slowing_factor: 5.0
60 RotateToGoal.lookahead_time: -1.0
61
62
63local_costmap:
64 local_costmap:
65 ros__parameters:
66 update_frequency: 5.0
67 publish_frequency: 2.0
68 global_frame: odom
69 robot_base_frame: base_link
70 use_sim_time: True
71 rolling_window: True
72 width: 3
73 height: 3
74 resolution: 0.06
75 robot_radius: 0.175
76 plugins: ["obstacle_layer", "inflation_layer"]
77 always_send_full_costmap: True
78 inflation_layer:
79 plugin: "nav2_costmap_2d::InflationLayer"
80 cost_scaling_factor: 4.0
81 inflation_radius: 0.45
82 obstacle_layer:
83 plugin: "nav2_costmap_2d::ObstacleLayer"
84 enabled: True
85 observation_sources: scan_source
86 scan_source:
87 topic: /scan
88 max_obstacle_height: 2.0
89 clearing: True
90 marking: True
91 data_type: "LaserScan"
92 raytrace_max_range: 3.0
93 raytrace_min_range: 0.0
94 obstacle_max_range: 2.5
95 obstacle_min_range: 0.0
Adding a Controller to a Launch File
Finally, we need to add the node to the launch file, along with including the config file and ensuring the lifecycle manager knows to handle it. Open the nav_demo.launch.py file and add the following lines.
1from ament_index_python.packages import get_package_share_directory
2from launch import LaunchDescription
3from launch.actions import IncludeLaunchDescription
4from launch_ros.actions import SetParameter, Node
5from launch.launch_description_sources import PythonLaunchDescriptionSource
6from launch.substitutions import PathJoinSubstitution
7
8
9def generate_launch_description():
10 ld = LaunchDescription()
11
12 # Parameters, Nodes and Launch files go here
13
14 # Declare package directory
15 pkg_nav_demos = get_package_share_directory('navigation_demos')
16 # Necessary fixes
17 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
18
19 lifecycle_nodes = [
20 'controller_server',
21 'planner_server',
22 'behaviour_server',
23 'bt_navigator',
24 ]
25
26 # LOAD PARAMETERS FROM YAML FILES
27 config_bt_nav = PathJoinSubstitution([pkg_nav_demos, 'config', 'bt_nav.yaml'])
28 config_planner = PathJoinSubstitution([pkg_nav_demos, 'config', 'planner.yaml'])
29 config_controller = PathJoinSubstitution([pkg_nav_demos, 'config', 'controller.yaml'])
30
31 # Include Gazebo Simulation
32 launch_gazebo = IncludeLaunchDescription(
33 PythonLaunchDescriptionSource([get_package_share_directory('gz_example_robot_description'), '/launch', '/sim_robot.launch.py']),
34 launch_arguments={}.items(),
35 )
36
37 # Include SLAM Toolbox standard launch file
38 launch_slamtoolbox = IncludeLaunchDescription(
39 PythonLaunchDescriptionSource([get_package_share_directory('slam_toolbox'), '/launch', '/online_async_launch.py']),
40 launch_arguments={}.items(),
41 )
42
43 # Behaviour Tree Navigator
44 node_bt_nav = Node(
45 package='nav2_bt_navigator',
46 executable='bt_navigator',
47 name='bt_navigator',
48 output='screen',
49 parameters=[config_bt_nav],
50 remappings=remappings,
51 )
52
53 # Behaviour Tree Server
54 node_behaviour = Node(
55 package='nav2_behaviors',
56 executable='behavior_server',
57 name='behaviour_server',
58 output='screen',
59 parameters=[config_bt_nav],
60 remappings=remappings,
61 )
62
63 # Planner Server Node
64 node_planner = Node(
65 package='nav2_planner',
66 executable='planner_server',
67 name='planner_server',
68 output='screen',
69 parameters=[config_planner],
70 remappings=remappings,
71 )
72
73 # Controller Server Node
74 node_controller = Node(
75 package='nav2_controller',
76 executable='controller_server',
77 name='controller_server',
78 output='screen',
79 parameters=[config_controller],
80 remappings=remappings,
81 )
82
83 # Lifecycle Node Manager to automatically start lifecycles nodes (from list)
84 node_lifecycle_manager = Node(
85 package='nav2_lifecycle_manager',
86 executable='lifecycle_manager',
87 name='lifecycle_manager_navigation',
88 output='screen',
89 parameters=[{'autostart': True}, {'node_names': lifecycle_nodes}],
90 )
91
92
93 # Add actions to LaunchDescription
94 ld.add_action(SetParameter(name='use_sim_time', value=True))
95 ld.add_action(launch_gazebo)
96 ld.add_action(launch_slamtoolbox)
97 ld.add_action(node_bt_nav)
98 ld.add_action(node_behaviour)
99 ld.add_action(node_planner)
100 ld.add_action(node_controller)
101 ld.add_action(node_lifecycle_manager)
102
103 return ld
Perform the usual colcon build, source install/setup.bash and check the launch file runs.