Update nav2_params.yaml

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_to_pose_action_bt_node
-    - nav2_compute_path_through_poses_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
-    - nav2_smooth_path_action_bt_node
+      - nav2_smooth_path_action_bt_node
-    - nav2_follow_path_action_bt_node
+      - nav2_follow_path_action_bt_node
-    - nav2_spin_action_bt_node
+      - nav2_spin_action_bt_node
-    - nav2_wait_action_bt_node
+      - nav2_wait_action_bt_node
-    - nav2_assisted_teleop_action_bt_node
+      - nav2_assisted_teleop_action_bt_node
-    - nav2_back_up_action_bt_node
+      - nav2_back_up_action_bt_node
-    - nav2_drive_on_heading_bt_node
+      - nav2_drive_on_heading_bt_node
-    - nav2_clear_costmap_service_bt_node
+      - nav2_clear_costmap_service_bt_node
-    - nav2_is_stuck_condition_bt_node
+      - nav2_is_stuck_condition_bt_node
-    - nav2_goal_reached_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
-    - nav2_goal_updated_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
-    - nav2_globally_updated_goal_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
-    - nav2_is_path_valid_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
-    - nav2_initial_pose_received_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
-    - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
-    - nav2_rate_controller_bt_node
+      - nav2_rate_controller_bt_node
-    - nav2_distance_controller_bt_node
+      - nav2_distance_controller_bt_node
-    - nav2_speed_controller_bt_node
+      - nav2_speed_controller_bt_node
-    - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_action_bt_node
-    - nav2_truncate_path_local_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
-    - nav2_goal_updater_node_bt_node
+      - nav2_goal_updater_node_bt_node
-    - nav2_recovery_node_bt_node
+      - nav2_recovery_node_bt_node
-    - nav2_pipeline_sequence_bt_node
+      - nav2_pipeline_sequence_bt_node
-    - nav2_round_robin_node_bt_node
+      - nav2_round_robin_node_bt_node
-    - nav2_transform_available_condition_bt_node
+      - nav2_transform_available_condition_bt_node
-    - nav2_time_expired_condition_bt_node
+      - nav2_time_expired_condition_bt_node
-    - nav2_path_expiring_timer_condition
+      - nav2_path_expiring_timer_condition
-    - nav2_distance_traveled_condition_bt_node
+      - nav2_distance_traveled_condition_bt_node
-    - nav2_single_trigger_bt_node
+      - nav2_single_trigger_bt_node
-    - nav2_goal_updated_controller_bt_node
+      - nav2_goal_updated_controller_bt_node
-    - nav2_is_battery_low_condition_bt_node
+      - nav2_is_battery_low_condition_bt_node
-    - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_through_poses_action_bt_node
-    - nav2_navigate_to_pose_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
-    - nav2_remove_passed_goals_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
-    - nav2_planner_selector_bt_node
+      - nav2_planner_selector_bt_node
-    - nav2_controller_selector_bt_node
+      - nav2_controller_selector_bt_node
-    - nav2_goal_checker_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
-    - nav2_controller_cancel_bt_node
+      - nav2_controller_cancel_bt_node
-    - nav2_path_longer_on_approach_bt_node
+      - nav2_path_longer_on_approach_bt_node
-    - nav2_wait_cancel_bt_node
+      - nav2_wait_cancel_bt_node
-    - nav2_spin_cancel_bt_node
+      - nav2_spin_cancel_bt_node
-    - nav2_back_up_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
-    - nav2_assisted_teleop_cancel_bt_node
+      - nav2_assisted_teleop_cancel_bt_node
-    - nav2_drive_on_heading_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_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older
-    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["goal_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
+    goal_checker:
-    #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
+      xy_goal_tolerance: 0.25 # was 0.25
-      yaw_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25 # was 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"]
+    FollowPath:
-      BaseObstacle.scale: 0.02
+      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
-      PathAlign.scale: 32.0
+      desired_linear_vel: 0.5
-      PathAlign.forward_point_distance: 0.1
+      lookahead_dist: 0.6
-      GoalAlign.scale: 24.0
+      min_lookahead_dist: 0.3
-      GoalAlign.forward_point_distance: 0.1
+      max_lookahead_dist: 0.9
-      PathDist.scale: 32.0
+      lookahead_time: 1.5
-      GoalDist.scale: 24.0
+      rotate_to_heading_angular_vel: 1.8
-      RotateToGoal.scale: 32.0
+      transform_tolerance: 0.1
-      RotateToGoal.slowing_factor: 5.0
+      use_velocity_scaled_lookahead_dist: false
-      RotateToGoal.lookahead_time: -1.0
+      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"
+      plugin: "nav2_smac_planner/SmacPlanner2D"
-      tolerance: 0.5
+      tolerance: 0.125                      # tolerance for planning if unable to reach exact pose, in meters
-      use_astar: false
+      downsample_costmap: false             # whether or not to downsample the map
-      allow_unknown: true
+      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_topic: "/odom
-    odom_duration: 0.1
+    odom_duration: 0.1 # was 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0