Costmap Filter Info Publisher Server publishes messages of nav2_msgs/CostmapFilterInfo type. These messages contain meta information of the costmap filter we use, like filter type or data conversion coefficients. Accordingly, we need to run this server as a lifecycle node.
Quote: According to the
design document,
nav2_msgs/CostmapFilterInfo messages are going in a pair
with OccupancyGrid filter mask topic. Therefore, along with
Costmap Filter Info Publisher Server there should be enabled a new instance of
Map Server configured to publish filter mask.
Here is an example (fragment) of localization_launch.py
Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
emulate_tty=True, # https://github.com/ros2/launch/issues/188
parameters=[configured_params],
remappings=remappings),
Node(
package='nav2_map_server',
executable='costmap_filter_info_server',
name='costmap_filter_info_server',
namespace=namespace,
output='screen',
emulate_tty=True, # https://github.com/ros2/launch/issues/188
parameters=[configured_params]),
In nav2_params.yaml file, we have to provide parameters for our Costmap Filter Info Publisher Server and Map Server nodes. For a complete list of parameters, refer to Map Server / Saver page.
nav2_params.yaml:
amcl:
ros__parameters:
set_initial_pose: True
initial_pose:
x: -8.13
y: -5.1
z: 0.0
yaw: 0.0
initial_pose: [-8.13, -5.1, 0.0, 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"
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
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 # /odom #/wheel/odometry
bt_loop_duration: 10
default_server_timeout: 20
enable_groot_monitoring: True
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
# Galactic:
default_bt_xml_filename: "navigate_to_pose_w_replanning_and_recovery.xml"
#Foxy:
#default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
plugin_lib_names:
# Galactic:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_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
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
# Foxy:
#- 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
#- nav2_single_trigger_bt_node
#- nav2_is_battery_low_condition_bt_node
#- nav2_navigate_through_poses_action_bt_node
#- nav2_navigate_to_pose_action_bt_node
#- nav2_remove_passed_goals_action_bt_node
#- nav2_planner_selector_bt_node
#- nav2_controller_selector_bt_node
#- nav2_goal_checker_selector_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker" # ["general_goal_checker"] # "precise_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
#precise_goal_checker:
# plugin: "nav2_controller::SimpleGoalChecker"
# xy_goal_tolerance: 0.25
# yaw_goal_tolerance: 0.25
# stateful: True
#general_goal_checker:
# stateful: True
# plugin: "nav2_controller::SimpleGoalChecker"
# xy_goal_tolerance: 0.25
# yaw_goal_tolerance: 0.25
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 1.0
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: 1.0
max_vel_y: 0.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 1.0
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
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"]
# UNCOMMENT FOR KEEPOUT DEMO
filters: ["keepout_filter"]
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
# UNCOMMENT FOR KEEPOUT DEMO
keepout_filter:
plugin: "nav2_costmap_2d::KeepoutFilter"
enabled: True
filter_info_topic: "/costmap_filter_info"
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"]
# UNCOMMENT FOR KEEPOUT DEMO
filters: ["keepout_filter"]
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
# UNCOMMENT FOR KEEPOUT DEMO
keepout_filter:
plugin: "nav2_costmap_2d::KeepoutFilter"
enabled: True
filter_info_topic: "/costmap_filter_info"
global_costmap_client:
ros__parameters:
use_sim_time: True
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
# UNCOMMENT FOR KEEPOUT DEMO
costmap_filter_info_server:
ros__parameters:
use_sim_time: true
type: 0
filter_info_topic: "/costmap_filter_info"
mask_topic: "/keepout_filter_mask"
base: 0.0
multiplier: 1.0
# UNCOMMENT FOR KEEPOUT DEMO
filter_mask_server:
ros__parameters:
use_sim_time: true
frame_id: "map"
topic_name: "/keepout_filter_mask"
yaml_filename: "$HOME/SnowCron/ros_projects/ROS2/harsh/src/maps/keepout_mask.yaml"
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: 20.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: false
allow_unknown: true
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", "backup", "wait"]
spin:
plugin: "nav2_recoveries/Spin"
backup:
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:
use_sim_time: True
# 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:
use_sim_time: True
loop_rate: 20
stop_on_failure: False
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 200
#lifecycle_manager:
# ros__parameters:
# use_sim_time: True
# 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
For Keepout Filter, the type should be set to 0, mask -topic should be equal to mask_topic parameter of Costmap Filter Info Publishing Server and topic_name parameter of Map Server.
According to official tutorial, base and multiplier are not used, but should be set to 0.0 and 1.0 nevertheless.