In this section we introduce a new file, config/nav2_params.yaml, that contains the configuration settings for our bot. This file is used by nav2_costmap_2d. In our example, we will use static layer, obstacle layer, voxel layer, and inflation layer. Both obstacle and voxel layers will use use the LaserScan messages published to the /scan topic by the lidar sensor.
We will also set some parameters to define how the detected obstacles are reflected in the costmap. For the complete list of layer plugin parameters, see the Costmap 2D Configuration Guide.
config/nav2_params.yaml:
amcl: ros__parameters: set_initial_pose: true initial_pose: x: 0.0 y: 0.0 z: 0.0 yaw: 0.0 use_sim_time: true alpha1: 0.002 alpha2: 0.002 alpha3: 0.002 alpha4: 0.002 alpha5: 0.002 base_frame_id: "base_footprint" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 do_beamskip: false global_frame_id: "map" initial_pose: [0.0, 0.0, 0.0, 0.0] lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 100.0 laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 max_particles: 2000 min_particles: 500 odom_frame_id: "odom" pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 robot_model_type: "differential" save_pose_rate: 0.5 set_initial_pose: false sigma_hit: 0.2 tf_broadcast: true transform_tolerance: 1.0 update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 z_max: 0.05 z_rand: 0.5 z_short: 0.05 scan_topic: scan amcl_map_client: ros__parameters: use_sim_time: true amcl_rclcpp_node: ros__parameters: use_sim_time: true bt_navigator: ros__parameters: use_sim_time: true global_frame: map robot_base_frame: base_link odom_topic: /odometry/filtered enable_groot_monitoring: true groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node - nav2_goal_updated_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node - nav2_speed_controller_bt_node - nav2_truncate_path_action_bt_node - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node bt_navigator_rclcpp_node: ros__parameters: use_sim_time: true controller_server: ros__parameters: use_sim_time: true controller_frequency: 2.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.5 movement_time_allowance: 10.0 # Goal checker parameters goal_checker: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: true # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true min_vel_x: 0.0 min_vel_y: 0.0 # Default 0.26 for max_vel_x and max_speed_xy and 1.0 for max_vel_theta max_vel_x: 0.26 max_vel_y: 0.0 max_vel_theta: 1.0 min_speed_xy: 0.0 max_speed_xy: 0.26 min_speed_theta: 0.0 # Add high threshold velocity for turtlebot 3 issue. # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 acc_lim_x: 2.5 acc_lim_y: 0.0 acc_lim_theta: 3.2 decel_lim_x: -2.5 decel_lim_y: 0.0 decel_lim_theta: -3.2 vx_samples: 20 vy_samples: 5 vtheta_samples: 20 sim_time: 1.7 linear_granularity: 0.05 angular_granularity: 0.025 transform_tolerance: 0.2 xy_goal_tolerance: 0.25 trans_stopped_velocity: 0.25 short_circuit_trajectory_evaluation: true stateful: true critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] BaseObstacle.scale: 0.02 PathAlign.scale: 32.0 PathAlign.forward_point_distance: 0.1 GoalAlign.scale: 24.0 GoalAlign.forward_point_distance: 0.1 PathDist.scale: 32.0 GoalDist.scale: 24.0 RotateToGoal.scale: 32.0 RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 controller_server_rclcpp_node: ros__parameters: use_sim_time: true local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link use_sim_time: true rolling_window: true width: 10 height: 10 resolution: 0.05 robot_radius: 0.75 plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.95 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: true publish_voxel_map: true origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 max_obstacle_height: 2.0 mark_threshold: 0 observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 clearing: true marking: true data_type: "LaserScan" static_layer: map_subscribe_transient_local: true always_send_full_costmap: true local_costmap_client: ros__parameters: use_sim_time: true local_costmap_rclcpp_node: ros__parameters: use_sim_time: true global_costmap: global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map robot_base_frame: base_link use_sim_time: true robot_radius: 0.75 resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_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" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: true inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.95 always_send_full_costmap: true global_costmap_client: ros__parameters: use_sim_time: true global_costmap_rclcpp_node: ros__parameters: use_sim_time: true map_server: ros__parameters: use_sim_time: true yaml_filename: "map.yaml" topic_name: "map" frame_id: "map" map_saver: ros__parameters: use_sim_time: true save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: false planner_server: ros__parameters: expected_planner_frequency: 2.0 use_sim_time: true planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true costmap_filter_info_server: ros__parameters: type: 1 filter_info_topic: "costmap_filter_info" mask_topic: "filter_mask" base: 0.0 multiplier: 0.25 planner_server_rclcpp_node: ros__parameters: use_sim_time: true recoveries_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 recovery_plugins: ["spin", "back_up", "wait"] spin: plugin: "nav2_recoveries/Spin" back_up: plugin: "nav2_recoveries/BackUp" wait: plugin: "nav2_recoveries/Wait" global_frame: odom robot_base_frame: base_link transform_timeout: 0.1 use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 robot_state_publisher: ros__parameters: use_sim_time: true slam_toolbox: ros__parameters: # Plugin params solver_plugin: solver_plugins::CeresSolver ceres_linear_solver: SPARSE_NORMAL_CHOLESKY ceres_preconditioner: SCHUR_JACOBI ceres_trust_strategy: LEVENBERG_MARQUARDT ceres_dogleg_type: TRADITIONAL_DOGLEG ceres_loss_function: None # ROS Parameters odom_frame: odom map_frame: map base_frame: base_footprint scan_topic: /scan mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose # or at the dock, but they are mutually exclusive, if pose is given # will use pose #map_file_name: test_steve #map_start_pose: [0.0, 0.0, 0.0] #map_start_at_dock: true debug_logging: false throttle_scans: 1 transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 5.0 resolution: 0.05 max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 tf_buffer_duration: 30. stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps enable_interactive_mode: true # General Parameters use_scan_matching: true use_scan_barycenter: true minimum_travel_distance: 0.5 minimum_travel_heading: 0.5 scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 10.0 link_match_minimum_response_fine: 0.1 link_scan_maximum_distance: 1.5 loop_search_maximum_distance: 3.0 do_loop_closing: true loop_match_minimum_chain_size: 10 loop_match_maximum_variance_coarse: 3.0 loop_match_minimum_response_coarse: 0.35 loop_match_minimum_response_fine: 0.45 # Correlation Parameters - Correlation Parameters correlation_search_space_dimension: 0.5 correlation_search_space_resolution: 0.01 correlation_search_space_smear_deviation: 0.1 # Correlation Parameters - Loop Closure Parameters loop_search_space_dimension: 8.0 loop_search_space_resolution: 0.05 loop_search_space_smear_deviation: 0.03 # Scan Matcher Parameters distance_variance_penalty: 0.5 angle_variance_penalty: 1.0 fine_search_angle_offset: 0.00349 coarse_search_angle_offset: 0.349 coarse_angle_resolution: 0.0349 minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 use_response_expansion: true waypoint_follower: ros__parameters: loop_rate: 1 stop_on_failure: false lifecycle_manager: ros__parameters: autostart: true node_names: ['nav2_map_server', 'controller_server', 'planner_server', 'behavior_server', 'bt_navigator', 'waypoint_follower'] bond_timeout: 4.0 attempt_respawn_reconnection: true bond_respawn_max_duration: 10.0
Notice that we have two two different costmaps here: global_costmap and local_costmap. The global_costmap is mainly used for long-term planning over the whole map while local_costmap is for short-term planning and collision avoidance.
The layers that we want to use are defined in the plugins parameter, both for global_costmap and local_costmap:
local_costmap: ... plugins: ["voxel_layer", "inflation_layer"]
These values list the mapped layer names, and also serve as namespaces for the layer parameters:
plugins: ["voxel_layer", "inflation_layer"] inflation_layer: ... voxel_layer: ...
Note that each layer/namespace in this list must have a plugin parameter defining the type of plugin to be loaded for that specific layer.
For the static layer, we set the map_subscribe_transient_local parameter to True. This sets the QoS settings for the map topic.
Another important parameter for the static layer is the map_topic which defines the map topic to subscribe to. This defaults to /map topic when not defined.
For the obstacle layer, we define its sensor source under the observation_sources parameter as scan (and on the following lines, we set its parameters). We set its topic parameter as the topic that publishes the defined sensor source and we set the data_type according to the sensor source it will use. In our configuration, the obstacle layer will use the LaserScan published by the lidar sensor to /scan.
Note that the obstacle layer and voxel layer can use either or both LaserScan and PointCloud2 as their data_type but it is set to LaserScan by default. The code snippet below shows an example of using both the LaserScan and PointCloud2 as the sensor sources. This may be particularly useful when setting up your own physical robot.
obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan pointcloud scan: topic: /scan data_type: "LaserScan" pointcloud: topic: /depth_camera/points data_type: "PointCloud2"
For the other parameters of the obstacle layer, the max_obstacle_height parameter sets the maximum height of the sensor reading to return to the occupancy grid. The minimum height of the sensor reading can also be set using the min_obstacle_height parameter, which defaults to 0 since we did not set it in the configation.
The clearing parameter is used to set whether the obstacle is to be removed from the costmap or not. The clearing operation is done by raytracing through the grid.
The maximum and minimum range to raytrace clear objects from the costmap is set using the raytrace_max_range and raytrace_min_range respectively.
The marking parameter is used to set whether the inserted obstacle is marked into the costmap or not.
We also set the maximum and minimum range to mark obstacles in the costmap through the obstacle_max_range and obstacle_min_range respectively.
For the inflation layer, we set the exponential decay factor across the inflation radius using the cost_scaling_factor parameter. The value of the radius to inflate around lethal obstacles is defined using the inflation_radius.
For the voxel layer, we set the publish_voxel_map parameter to True to enable the publishing of the 3D voxel grid. The resolution of the voxels in height is defined using the z_resolution parameter, while the number of voxels in each column is defined using the z_voxels parameter.
The mark_threshold parameter sets the minimum number of voxels in a column to mark as occupied in the occupancy grid. We set the observation_sources parameter of the voxel layer to scan, and we set the scan parameters similar to the parameters that we have discussed for the obstacle layer. As defined in its topic and data_type parameters, the voxel layer will use the LaserScan published on the /scan topic by the lidar scanner.
Note that the we are not using a range layer for our configuration but it may be useful for your own robot setup. For the range layer, its basic parameters are the topics, input_sensor_type, and clear_on_max_reading parameters. The range topics to subscribe to are defined in the topics parameter. The input_sensor_type is set to either ALL, VARIABLE, or FIXED. The clear_on_max_reading is a boolean parameter that sets whether to clear the sensor readings on max range.