diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 36d662fc754..2027f4c0e1c 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -276,265 +276,14 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased", "DEFAULT_Smac2d", "BIDIRECTIONAL_Smac2d", "ALL_DIRECTION_Smac2d", "DEFAULT_SmacHybrid", "BIDIRECTIONAL_SmacHybrid", "ALL_DIRECTION_SmacHybrid", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacLattice"] + planner_plugins: ["GridBased"] costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true - DEFAULT_Smac2d: - plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - BIDIRECTIONAL_Smac2d: - plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - goal_heading_mode: "BIDIRECTIONAL" - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - ALL_DIRECTION_Smac2d: - plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - goal_heading_mode: "ALL_DIRECTIONAL" - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - - DEFAULT_SmacHybrid: - plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # max time in s for planner to plan, smooth - motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp - angle_quantization_bins: 72 # Number of angle bins for search - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - retrospective_penalty: 0.015 - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. - use_quadratic_cost_penalty: False - downsample_obstacle_heuristic: True - allow_primitive_interpolation: False - # goal_heading_mode: "BIDIRECTIONAL" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - BIDIRECTIONAL_SmacHybrid: - plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # max time in s for planner to plan, smooth - motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp - angle_quantization_bins: 72 # Number of angle bins for search - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - retrospective_penalty: 0.015 - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. - use_quadratic_cost_penalty: False - downsample_obstacle_heuristic: True - allow_primitive_interpolation: False - goal_heading_mode: "BIDIRECTIONAL" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - ALL_DIRECTION_SmacHybrid: - plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # max time in s for planner to plan, smooth - motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp - angle_quantization_bins: 72 # Number of angle bins for search - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - retrospective_penalty: 0.015 - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. - use_quadratic_cost_penalty: False - downsample_obstacle_heuristic: True - allow_primitive_interpolation: False - goal_heading_mode: "ALL_DIRECTION" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - - DEFAULT_SmacLattice: - plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" - allow_unknown: true # Allow traveling in unknown space - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - # lattice_filepath: "" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - BIDIRECTIONAL_SmacLattice: - plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" - allow_unknown: true # Allow traveling in unknown space - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - # lattice_filepath: "" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - goal_heading_mode: "BIDIRECTIONAL" - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - ALL_DIRECTION_SmacLattice: - plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" - allow_unknown: true # Allow traveling in unknown space - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - # lattice_filepath: "" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - goal_heading_mode: "ALL_DIRECTION" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 smoother_server: ros__parameters: smoother_plugins: ["simple_smoother"] diff --git a/nav2_bringup/params/nav2_params.yaml.backup b/nav2_bringup/params/nav2_params.yaml.backup deleted file mode 100644 index 2027f4c0e1c..00000000000 --- a/nav2_bringup/params/nav2_params.yaml.backup +++ /dev/null @@ -1,431 +0,0 @@ -amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - 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: "nav2_amcl::DifferentialMotionModel" - 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 - -bt_navigator: - ros__parameters: - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - costmap_update_timeout: 0.30 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] - use_realtime_priority: false - - # 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 - FollowPath: - plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 - model_dt: 0.05 - batch_size: 2000 - ax_max: 3.0 - ax_min: -3.0 - ay_max: 3.0 - az_max: 3.5 - vx_std: 0.2 - vy_std: 0.2 - wz_std: 0.4 - vx_max: 0.5 - vx_min: -0.35 - vy_max: 0.5 - wz_max: 1.9 - iteration_count: 1 - prune_distance: 1.7 - transform_tolerance: 0.1 - temperature: 0.3 - gamma: 0.015 - motion_model: "DiffDrive" - visualize: true - regenerate_noises: true - TrajectoryVisualizer: - trajectory_step: 5 - time_step: 3 - AckermannConstraints: - min_turning_r: 0.2 - critics: [ - "ConstraintCritic", "CostCritic", "GoalCritic", - "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", - "PathAngleCritic", "PreferForwardCritic"] - ConstraintCritic: - enabled: true - cost_power: 1 - cost_weight: 4.0 - GoalCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 1.4 - GoalAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 3.0 - threshold_to_consider: 0.5 - PreferForwardCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 0.5 - CostCritic: - enabled: true - cost_power: 1 - cost_weight: 3.81 - critical_cost: 300.0 - consider_footprint: true - collision_cost: 1000000.0 - near_goal_distance: 1.0 - trajectory_point_step: 2 - PathAlignCritic: - enabled: true - cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 - use_path_orientations: false - PathFollowCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 - PathAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 2.0 - offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 - mode: 0 - # TwirlingCritic: - # enabled: true - # twirling_cost_power: 1 - # twirling_cost_weight: 10.0 - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - rolling_window: true - width: 3 - height: 3 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.70 - 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" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - robot_radius: 0.22 - 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" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - 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.7 - always_send_full_costmap: True - -# The yaml_filename does not need to be specified since it going to be set by defaults in launch. -# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py -# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. -# map_server: -# ros__parameters: -# yaml_filename: "" - -map_saver: - ros__parameters: - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: True - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] - costmap_update_timeout: 1.0 - GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true - -smoother_server: - ros__parameters: - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - local_costmap_topic: local_costmap/costmap_raw - global_costmap_topic: global_costmap/costmap_raw - local_footprint_topic: local_costmap/published_footprint - global_footprint_topic: global_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - assisted_teleop: - plugin: "nav2_behaviors::AssistedTeleop" - local_frame: odom - global_frame: map - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 2.0] - min_velocity: [-0.5, 0.0, -2.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 1.2 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "scan" - min_height: 0.15 - max_height: 2.0 - enabled: True - -docking_server: - ros__parameters: - controller_frequency: 50.0 - initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 - undock_linear_tolerance: 0.05 - undock_angular_tolerance: 0.1 - max_retries: 3 - base_frame: "base_link" - fixed_frame: "odom" - dock_backwards: false - dock_prestaging_tolerance: 0.5 - - # Types of docks - dock_plugins: ['simple_charging_dock'] - simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docking_threshold: 0.05 - staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 - filter_coef: 0.1 - - # Dock instances - # The following example illustrates configuring dock instances. - # docks: ['home_dock'] # Input your docks here - # home_dock: - # type: 'simple_charging_dock' - # frame: map - # pose: [0.0, 0.0, 0.0] - - controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 - -loopback_simulator: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - map_frame_id: "map" - scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' - update_duration: 0.02 diff --git a/nav2_bringup/params/nav2_params_default.yaml b/nav2_bringup/params/nav2_params_default.yaml deleted file mode 100644 index 698e27a69a0..00000000000 --- a/nav2_bringup/params/nav2_params_default.yaml +++ /dev/null @@ -1,510 +0,0 @@ -amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - 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: "nav2_amcl::DifferentialMotionModel" - 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 - -bt_navigator: - ros__parameters: - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - costmap_update_timeout: 0.30 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] - use_realtime_priority: false - - # 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 - FollowPath: - plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 - model_dt: 0.05 - batch_size: 2000 - ax_max: 3.0 - ax_min: -3.0 - ay_max: 3.0 - az_max: 3.5 - vx_std: 0.2 - vy_std: 0.2 - wz_std: 0.4 - vx_max: 0.5 - vx_min: -0.35 - vy_max: 0.5 - wz_max: 1.9 - iteration_count: 1 - prune_distance: 1.7 - transform_tolerance: 0.1 - temperature: 0.3 - gamma: 0.015 - motion_model: "DiffDrive" - visualize: true - regenerate_noises: true - TrajectoryVisualizer: - trajectory_step: 5 - time_step: 3 - AckermannConstraints: - min_turning_r: 0.2 - critics: [ - "ConstraintCritic", "CostCritic", "GoalCritic", - "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", - "PathAngleCritic", "PreferForwardCritic"] - ConstraintCritic: - enabled: true - cost_power: 1 - cost_weight: 4.0 - GoalCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 1.4 - GoalAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 3.0 - threshold_to_consider: 0.5 - PreferForwardCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 0.5 - CostCritic: - enabled: true - cost_power: 1 - cost_weight: 3.81 - critical_cost: 300.0 - consider_footprint: true - collision_cost: 1000000.0 - near_goal_distance: 1.0 - trajectory_point_step: 2 - PathAlignCritic: - enabled: true - cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 - use_path_orientations: false - PathFollowCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 - PathAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 2.0 - offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 - mode: 0 - # TwirlingCritic: - # enabled: true - # twirling_cost_power: 1 - # twirling_cost_weight: 10.0 - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - rolling_window: true - width: 3 - height: 3 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.70 - 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" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - robot_radius: 0.22 - 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" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - 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.7 - always_send_full_costmap: True - -# The yaml_filename does not need to be specified since it going to be set by defaults in launch. -# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py -# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. -# map_server: -# ros__parameters: -# yaml_filename: "" - -map_saver: - ros__parameters: - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: True - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - planner_plugins: ["Smac2d", "SmacHybrid", "SmacLattice"] - costmap_update_timeout: 1.0 - Smac2d: - plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - - - SmacHybrid: - plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # max time in s for planner to plan, smooth - motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp - angle_quantization_bins: 72 # Number of angle bins for search - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - retrospective_penalty: 0.015 - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. - use_quadratic_cost_penalty: False - downsample_obstacle_heuristic: True - allow_primitive_interpolation: False - # goal_heading_mode: "BIDIRECTIONAL" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - SmacLattice: - plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" - allow_unknown: true # Allow traveling in unknown space - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - # lattice_filepath: "" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - -smoother_server: - ros__parameters: - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - local_costmap_topic: local_costmap/costmap_raw - global_costmap_topic: global_costmap/costmap_raw - local_footprint_topic: local_costmap/published_footprint - global_footprint_topic: global_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - assisted_teleop: - plugin: "nav2_behaviors::AssistedTeleop" - local_frame: odom - global_frame: map - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 2.0] - min_velocity: [-0.5, 0.0, -2.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 1.2 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "scan" - min_height: 0.15 - max_height: 2.0 - enabled: True - -docking_server: - ros__parameters: - controller_frequency: 50.0 - initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 - undock_linear_tolerance: 0.05 - undock_angular_tolerance: 0.1 - max_retries: 3 - base_frame: "base_link" - fixed_frame: "odom" - dock_backwards: false - dock_prestaging_tolerance: 0.5 - - # Types of docks - dock_plugins: ['simple_charging_dock'] - simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docking_threshold: 0.05 - staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 - filter_coef: 0.1 - - # Dock instances - # The following example illustrates configuring dock instances. - # docks: ['home_dock'] # Input your docks here - # home_dock: - # type: 'simple_charging_dock' - # frame: map - # pose: [0.0, 0.0, 0.0] - - controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 - -loopback_simulator: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - map_frame_id: "map" - scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' - update_duration: 0.02