amcl: ros__parameters: use_sim_time: False # Seed at the spawn pose (map origin) so Nav2 can activate at startup. beluga_amcl is 2D (x, y, yaw). set_initial_pose: true initial_pose: x: 0.0 y: 0.0 yaw: 0.0 covariance_x: 0.25 covariance_y: 0.25 covariance_yaw: 0.0685 alpha1: 0.1 alpha2: 0.1 alpha3: 0.1 alpha4: 0.1 alpha5: 0.1 base_frame_id: "footprint" global_frame_id: "map" lambda_short: 0.1 laser_likelihood_max_dist: 2.0 # Fuse is 2D, so on slopes the scan planes strike the ground; past 5 m those strikes smeared slam_toolbox's map over two laps. laser_max_range: 5.0 laser_min_range: 0.0 laser_model_type: "likelihood_field" max_beams: 725 max_particles: 5000 min_particles: 1000 odom_frame_id: "odom" pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.1 recovery_alpha_slow: 0.001 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.05 update_min_d: 0.25 z_hit: 0.5 z_max: 0.05 z_rand: 0.5 z_short: 0.05 # Both filtered scans, relayed here by robot_drivers_to_persist_sim.launch.py. scan_topic: /scan_interleaved bt_navigator: ros__parameters: use_sim_time: False global_frame: map robot_base_frame: footprint odom_topic: /odom_filtered bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 # Since Jazzy, bt_navigator loads the stock nav2 BT plugins implicitly; listing them in plugin_lib_names double-registers them. bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: use_sim_time: False bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: False controller_server: ros__parameters: use_sim_time: False # platform_velocity_controller takes TwistStamped. enable_stamped_cmd_vel: true odom_topic: /odom_filtered 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"] controller_plugins: ["FollowPath"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.1 movement_time_allowance: 20.0 general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.4 yaw_goal_tolerance: 0.25 FollowPath: plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 model_dt: 0.05 batch_size: 500 vx_std: 0.2 wz_std: 0.4 vx_max: 0.5 vx_min: -0.35 # Skid-steer odometry over-reports distance by ~13% in arcs at 0.3 rad/s, and tighter turns stall on the regolith (measured in MuJoCo across the field), so turns stay gentle. wz_max: 0.3 iteration_count: 1 # Seed each rollout from the last command rather than lagging odometry, which otherwise pins the commanded speed near a third of vx_max on Jazzy (moveit_pro issue #21202). Needs nav2_mppi_controller >= 1.3.13, which the workspace Dockerfile installs. open_loop: true prune_distance: 1.7 transform_tolerance: 0.1 temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" # Makes MPPI publish /optimal_trajectory, which the Desktop App draws as the Local Plan overlay. visualize: true TrajectoryVisualizer: trajectory_step: 5 time_step: 3 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 PathAlignCritic: enabled: true cost_power: 1 cost_weight: 14.0 max_path_occupancy_ratio: 0.05 trajectory_point_step: 3 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 forward_preference: true local_costmap: local_costmap: ros__parameters: update_frequency: 10.0 publish_frequency: 5.0 global_frame: odom robot_base_frame: footprint use_sim_time: False rolling_window: true width: 6 height: 6 resolution: 0.05 # Measured from husky_a300.xml: bumpers reach x = +-0.495 m, wheels and arch y = +-0.35 m. footprint: "[[0.50, 0.35], [0.50, -0.35], [-0.50, -0.35], [-0.50, 0.35]]" footprint_padding: 0.025 plugins: ["front_obstacle_layer", "rear_obstacle_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.75 # One layer per scan, so each clears only its own marks: a rear ray passing over a # landmark only the front plane reaches would otherwise erase it. front_obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan_front scan_front: topic: /scan_front_filtered max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" raytrace_max_range: 4.5 raytrace_min_range: 0.0 obstacle_max_range: 3.5 obstacle_min_range: 0.0 transform_tolerance: 0.3 inf_is_valid: false qos_overriding_options: policy_kinds: [reliability] reliability: best_effort rear_obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan_rear scan_rear: topic: /scan_rear_filtered max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" raytrace_max_range: 4.5 raytrace_min_range: 0.0 obstacle_max_range: 3.5 obstacle_min_range: 0.0 transform_tolerance: 0.3 inf_is_valid: false qos_overriding_options: policy_kinds: [reliability] reliability: best_effort always_send_full_costmap: True transform_tolerance: 0.3 global_costmap: global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map robot_base_frame: footprint use_sim_time: False # No saved map yet: a 20 x 20 m rolling window over the 30 x 30 m drivable field, built from the lidars alone. rolling_window: true width: 20 height: 20 footprint: "[[0.50, 0.35], [0.50, -0.35], [-0.50, -0.35], [-0.50, 0.35]]" footprint_padding: 0.025 resolution: 0.05 track_unknown_space: false plugins: ["front_obstacle_layer", "rear_obstacle_layer", "inflation_layer"] # One layer per scan, so each clears only its own marks: a rear ray passing over a # landmark only the front plane reaches would otherwise erase it. front_obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan_front scan_front: topic: /scan_front_filtered max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" raytrace_max_range: 6.0 raytrace_min_range: 0.0 obstacle_max_range: 5.0 obstacle_min_range: 0.0 transform_tolerance: 0.3 inf_is_valid: false qos_overriding_options: policy_kinds: [reliability] reliability: best_effort rear_obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan_rear scan_rear: topic: /scan_rear_filtered max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" raytrace_max_range: 6.0 raytrace_min_range: 0.0 obstacle_max_range: 5.0 obstacle_min_range: 0.0 transform_tolerance: 0.3 inf_is_valid: false qos_overriding_options: policy_kinds: [reliability] reliability: best_effort inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 4.0 inflation_radius: 0.75 always_send_full_costmap: True transform_tolerance: 0.3 map_server: ros__parameters: use_sim_time: False # Set by robot_drivers_to_persist_sim.launch.py from its "map" launch argument. yaml_filename: "" map_saver: ros__parameters: use_sim_time: False 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: False 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: False smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" tolerance: 1.0e-10 max_its: 1000 do_refinement: True behavior_server: ros__parameters: enable_stamped_cmd_vel: true 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: footprint transform_tolerance: 0.1 use_sim_time: False simulate_ahead_time: 2.0 max_rotational_vel: 0.3 min_rotational_vel: 0.1 rotational_acc_lim: 3.2 waypoint_follower: ros__parameters: use_sim_time: False 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: False enable_stamped_cmd_vel: true smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" # x, y, yaw. Within platform_velocity_controller's limits; y is zero for this skid-steer base. max_velocity: [0.5, 0.0, 0.3] min_velocity: [-0.35, 0.0, -0.3] max_accel: [2.0, 0.0, 3.2] max_decel: [-2.0, 0.0, -3.2] odom_topic: "/odom_filtered" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 slam_toolbox: ros__parameters: use_sim_time: False 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 odom_frame: odom map_frame: map base_frame: footprint scan_topic: /scan_front_filtered mode: mapping debug_logging: false throttle_scans: 1 transform_publish_period: 0.02 map_update_interval: 5.0 resolution: 0.05 # Same ground-strike limit as amcl.laser_max_range. max_laser_range: 5.0 minimum_time_interval: 0.5 tf_buffer_duration: 30.0 stack_size_to_use: 40000000 enable_interactive_mode: true # Skid-steer wheel odometry drifts on regolith, so let scan matching and loop closure correct it. use_scan_matching: true use_scan_barycenter: true minimum_travel_distance: 0.3 minimum_travel_heading: 0.3 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_search_space_dimension: 0.5 correlation_search_space_resolution: 0.01 correlation_search_space_smear_deviation: 0.1 loop_search_space_dimension: 8.0 loop_search_space_resolution: 0.05 loop_search_space_smear_deviation: 0.03 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