From bf049490c34a6626859fbd5deb9dd7f0acca8b4a Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Thu, 16 Nov 2023 14:21:31 +0100 Subject: [PATCH] Update nav2_params.yaml --- config/nav2_params.yaml | 732 +++++++++++++++++++++++++++++++++------- 1 file changed, 615 insertions(+), 117 deletions(-) diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml index 27ec277..41023d1 100644 --- a/config/nav2_params.yaml +++ b/config/nav2_params.yaml @@ -1,3 +1,430 @@ +# 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 +# #scan_topic: /scan_filtered +# use_map_saver: true +# 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 + +# amcl: +# ros__parameters: +# use_sim_time: True +# 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: +# use_sim_time: True +# global_frame: map +# robot_base_frame: base_link +# odom_topic: /odometry/filtered +# bt_loop_duration: 10 +# default_server_timeout: 20 +# # '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: +# - nav2_compute_path_to_pose_action_bt_node +# - nav2_compute_path_through_poses_action_bt_node +# - nav2_smooth_path_action_bt_node +# - nav2_follow_path_action_bt_node +# - nav2_spin_action_bt_node +# - nav2_wait_action_bt_node +# - nav2_assisted_teleop_action_bt_node +# - nav2_back_up_action_bt_node +# - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node +# - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition +# - nav2_distance_traveled_condition_bt_node +# - nav2_single_trigger_bt_node +# - nav2_goal_updated_controller_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 +# - nav2_controller_cancel_bt_node +# - nav2_path_longer_on_approach_bt_node +# - nav2_wait_cancel_bt_node +# - nav2_spin_cancel_bt_node +# - nav2_back_up_cancel_bt_node +# - nav2_assisted_teleop_cancel_bt_node +# - nav2_drive_on_heading_cancel_bt_node + +# bt_navigator_navigate_through_poses_rclcpp_node: +# ros__parameters: +# use_sim_time: True + +# bt_navigator_navigate_to_pose_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_plugins: ["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 +# # DWB parameters +# FollowPath: +# plugin: "dwb_core::DWBLocalPlanner" +# debug_trajectory_details: True +# min_vel_x: 0.0 +# min_vel_y: 0.0 +# 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 + +# 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: 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.55 +# 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 +# use_sim_time: True +# 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.55 +# always_send_full_costmap: True + +# map_server: +# ros__parameters: +# use_sim_time: True +# # Overridden in launch by the "map" launch configuration or provided default value. +# # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. +# yaml_filename: "" + +# map_saver: +# ros__parameters: +# use_sim_time: True +# 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 +# use_sim_time: True +# planner_plugins: ["GridBased"] +# GridBased: +# plugin: "nav2_navfn_planner/NavfnPlanner" +# tolerance: 0.5 +# use_astar: false +# allow_unknown: true + +# smoother_server: +# ros__parameters: +# use_sim_time: True +# smoother_plugins: ["simple_smoother"] +# simple_smoother: +# plugin: "nav2_smoother::SimpleSmoother" +# tolerance: 1.0e-10 +# max_its: 1000 +# do_refinement: True + +# behavior_server: +# ros__parameters: +# costmap_topic: local_costmap/costmap_raw +# footprint_topic: local_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" +# global_frame: odom +# robot_base_frame: base_link +# transform_tolerance: 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 + +# 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 + +# velocity_smoother: +# ros__parameters: +# use_sim_time: True +# smoothing_frequency: 20.0 +# scale_velocities: False +# feedback: "OPEN_LOOP" +# max_velocity: [0.26, 0.0, 1.0] +# min_velocity: [-0.26, 0.0, -1.0] +# max_accel: [2.5, 0.0, 3.2] +# max_decel: [-2.5, 0.0, -3.2] +# odom_topic: "/odometry/filtered" +# odom_duration: 0.1 +# deadband_velocity: [0.0, 0.0, 0.0] +# velocity_timeout: 1.0 + + + slam_toolbox: ros__parameters: @@ -90,7 +517,7 @@ amcl: global_frame_id: "map" lambda_short: 0.1 laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 + laser_max_range: 7.0 #was 100.0 laser_min_range: -1.0 laser_model_type: "likelihood_field" max_beams: 60 @@ -120,7 +547,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /odometry/filtered + odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -128,52 +555,53 @@ bt_navigator: # 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: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_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 - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_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 + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: @@ -183,76 +611,123 @@ bt_navigator_navigate_to_pose_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_plugins: ["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 +# # DWB parameters +# FollowPath: +# plugin: "dwb_core::DWBLocalPlanner" +# debug_trajectory_details: True +# min_vel_x: -0.1 +# min_vel_y: 0.0 +# 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", "ObstacleFootprint", "PreferForward"] +# 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: ros__parameters: use_sim_time: True - controller_frequency: 20.0 + controller_frequency: 100.0 # was 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_plugins: ["general_goal_checker"] # "precise_goal_checker" + progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older + goal_checker_plugins: ["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 + goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - 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 + xy_goal_tolerance: 0.25 # was 0.25 + yaw_goal_tolerance: 0.25 # was 0.25 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 + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.007 # was 0.05 + approach_velocity_scaling_dist: 0.6 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 0.25 + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: false # was true, cannot be set together with allow_reversing + allow_reversing: true # was false + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 + use_interpolation: true # was false + local_costmap: local_costmap: @@ -266,11 +741,12 @@ local_costmap: width: 3 height: 3 resolution: 0.05 - robot_radius: 0.22 + #robot_radius: 0.22 + footprint: "[ [0.18, 0.200], [0.18, -0.200], [-0.54, -0.165], [-0.54, 0.165] ]" # gave another points for the polygon "[ [0.18, 0.255], [0.18, -0.255], [-0.54, -0.165], [-0.54, 0.165] ]" plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 + cost_scaling_factor: 10.0 # was 3.0 inflation_radius: 0.55 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" @@ -305,7 +781,8 @@ global_costmap: global_frame: map robot_base_frame: base_link use_sim_time: True - robot_radius: 0.22 + #robot_radius: 0.22 + footprint: "[ [0.18, 0.255], [0.18, -0.255], [-0.54, -0.165], [-0.54, 0.165] ]" # gave another points for the polygon resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] @@ -328,7 +805,7 @@ global_costmap: map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 + cost_scaling_factor: 10.0 # was 3.0 inflation_radius: 0.55 always_send_full_costmap: True @@ -347,16 +824,37 @@ map_saver: occupied_thresh_default: 0.65 map_subscribe_transient_local: True +# 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: ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] + use_sim_time: True + GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + plugin: "nav2_smac_planner/SmacPlanner2D" + 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: true # 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: 0.1 smoother_server: ros__parameters: @@ -413,12 +911,12 @@ velocity_smoother: use_sim_time: True smoothing_frequency: 20.0 scale_velocities: False - feedback: "OPEN_LOOP" + feedback: "OPEN_LOOP" # was OPEN_LOOP max_velocity: [0.26, 0.0, 1.0] min_velocity: [-0.26, 0.0, -1.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] - odom_topic: "/odometry/filtered" - odom_duration: 0.1 + odom_topic: "/odom + odom_duration: 0.1 # was 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0