ROS2 Tutorial: Configuring Costmap 2D

Configuring nav2_costmap_2d

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.

(C) snowcron.com, all rights reserved

Please read the disclaimer