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_link scan_topic: scan_filtered 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: /home/bjorn/Documents/ros-projects/cps_bot_mini_ws/my_map_serial # 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: 1.0 #was 5.0 resolution: 0.05 max_laser_range: 12.0 #for rastering images, standard was 20 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: false # General Parameters use_scan_matching: true use_scan_barycenter: true minimum_travel_distance: 0.0 # was 0.5 minimum_travel_heading: 0.0 # was 0.5 scan_buffer_size: 20 # was 10 scan_buffer_maximum_scan_distance: 12.0 # was 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_link" 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: 12.0 #was 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_filtered 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 bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: base_link 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: # 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 # 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 #new progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older goal_checker_plugins: ["general_goal_checker"] # was "goal_checker" controller_plugins: ["FollowPath"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.5 movement_time_allowance: 10.0 general_goal_checker: # was goal_checker plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 # was 0.25 yaw_goal_tolerance: 0.25 # was 0.25 stateful: True 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 ## Changed to DWB again for Loki plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: False # was 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.06 # was 0.05 #robot_radius: 0.22 footprint: "[ [0.065, 0.160], [0.065, -0.160], [-0.28, -0.13], [-0.28, 0.13] ]" plugins: ["static_layer", "voxel_layer", "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 4.0 # was 10.0 inflation_radius: 0.45 # was 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: lslidar lslidar: topic: scan_filtered 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 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 footprint: "[ [0.065, 0.160], [0.065, -0.160], [-0.28, -0.13], [-0.28, 0.13] ]" resolution: 0.06 # was 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: lslidar lslidar: topic: scan_filtered 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: 4.0 # was 10.0 inflation_radius: 0.45 # was 0.55 always_send_full_costmap: True planner_server: ros__parameters: expected_planner_frequency: 20.0 # new planner_plugins: ["GridBased"] use_sim_time: True # GridBased: # 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 ## Changed back to Navfn planner for Loki 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" # 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: "odom" odom_duration: 0.1 # was 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0