bt_navigator: ros__parameters: global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 always_reload_bt_xml: true filter_duration: 0.3 default_server_timeout: 200 # original is 20 wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 service_introspection_mode: "disabled" navigators: ["navigate_to_pose", "navigate_through_poses", "navigate_route"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" enable_groot_monitoring: false groot_server_port: 1667 navigate_through_poses: plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" enable_groot_monitoring: false groot_server_port: 1669 navigate_route: plugin: "nav2_bt_navigator::NavigateRouteNavigator" enable_groot_monitoring: true groot_server_port: 1671 ## FIXME: remove hardcoded paths and use launch file remapping instead. The default BT XML files are in nav2_bt_navigator/behavior_trees. default_nav_to_pose_bt_xml: '/home/hyundai-motor/standard_amr_ws/overlay_ws/src/hmgics_ws/hmc_bringup/params/navigate_on_route_graph_stop_and_wait.xml' # default_nav_to_pose_bt_xml: '/home/hyundai-motor/standard_amr_ws/overlay_ws/src/hmgics_ws/hmgics_navigation2/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml' # default_nav_to_pose_bt_xml: '/home/hyundai-motor/standard_amr_ws/overlay_ws/src/hmgics_ws/hmgics_navigation2/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml' # '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_name_prefixes: - assisted_teleop - backup - compute_path - compute_route - dock_robot - drive_on_heading - follow_object - follow_path - nav_thru_poses - nav_to_pose - route - spin - smoother - undock_robot - wait controller_server: ros__parameters: use_sim_time: False controller_frequency: 20.0 costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.05 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker", "BaseGoal"] # controller_plugins: ["FollowPath", "FollowPathWithBaseController"] controller_plugins: ["FollowPath"] path_handler_plugins: ["PathHandler"] use_realtime_priority: false speed_limit_topic: "speed_limit" # Progress checker parameters progress_checker: plugin: "nav2_controller::PoseProgressChecker" required_movement_radius: 0.5 required_movement_angle: 0.5 movement_time_allowance: 10.0 # Goal checker parameters general_goal_checker: plugin: "nav2_controller::BidirectionalGoalChecker" xy_goal_tolerance: 0.08 ## in meter yaw_goal_tolerance: 0.0524 ## in radian, [02sep26] 0.0524 = 3 degree path_length_tolerance: 1.0 trans_stopped_velocity: 0.02 # m/s rot_stopped_velocity: 0.02 stateful: True # [NOTE] Disable this during deployment bypass_y_axis: False bypass_y_axis_max_x_error: 0.05 bypass_y_axis_max_y_error: 0.5 PathHandler: plugin: "nav2_controller::FeasiblePathHandler" prune_distance: 4.0 enforce_path_inversion: True enforce_path_rotation: False max_robot_pose_search_dist: 2.0 inversion_xy_tolerance: 0.2 inversion_yaw_tolerance: 0.4 minimum_rotation_angle: 0.785 reject_unit_path: False FollowPath: plugin: "nav2_rotation_shim_controller::RotationShimController" # --- shim --- angular_dist_threshold: 0.785 angular_disengage_threshold: 0.3925 forward_sampling_distance: 0.5 rotate_to_heading_angular_vel: 0.25 max_angular_accel: 0.35 max_cost_threshold: 254.0 simulate_ahead_time: 3.0 rotate_to_goal_heading: true rotate_to_heading_once: false closed_loop: true use_path_orientations: false bidirectional: true # vx_min is -1.0, PathAngleCritic mode is 1 # [HMGICS] goal_rotation_hysteresis: 1.5 primary_controller: "nav2_mppi_controller::MPPIController" # --- mppi --- time_steps: 70 model_dt: 0.05 model_delay_vx: 0.10 model_delay_vy: 0.0 model_delay_wz: 0.10 batch_size: 1600 ## acceleration ax_max: 3.0 ax_min: -3.0 ## linear velocity x vx_std: 0.5 #### 0.65 vx_max: 1.0 vx_min: -1.0 ## linear velocity y vy_std: 0.0 vy_max: 0.0 ay_max: 0.0 ay_min: 0.0 ## angular velocity wz_std: 0.25 #### 0.14 is the magic number woohoo wz_max: 1.2 #### NOTE: min 0.9 to turn smoothly or 2.0 corner az_max: 3.5 advanced: wz_std_decay_strength: 0.5 ## original 2.0 ### set -1.0 to disable wz_std_decay_to: 0.06 near_goal_velocity_scaling: true near_goal_distance: 1.0 near_goal_vx_max: 0.20 near_goal_vx_min: 0.0 near_goal_wz_max: 0.25 curvature_velocity_scaling: false max_lateral_accel: 0.17 curvature_decel: 1.0 curvature_sample_distance: 0.2 curvature_vx_min: 0.15 iteration_count: 1 transform_tolerance: 0.1 temperature: 0.30 gamma: 0.015 # motion_model: "DiffDrive" motion_model: "diff_drive" diff_drive: plugin: "mppi::DiffDriveMotionModel" visualize: true publish_optimal_trajectory: true publish_critics_stats: true # [DIAGNOSTIC] Draws the reference window the path-alignment critics can actually see # on ~/path_align_window, and adds the numbers to ~/critics_stats. PathAlignCritic # builds its reference only out to furthest_reached_path_point and clamps every # trajectory sample past it onto that one point, so when the green window is short # against the batch's reach the critic stops scoring alignment and starts scoring # SPEED -- measured, a 1.0 m window against a 3.85 m reach makes rollouts that are all # perfectly on the path spread ~44x more than the cost of being 10 cm off it, and the # cheapest trajectory in the batch becomes the slowest one. # # Watch path_window_length / trajectory_reach_length. Near 1.0 is healthy; the marker # label says DEGRADED below 0.8. publish_path_window: true # [NOTE] Disable this during deployment regenerate_noises: false TrajectoryVisualizer: trajectory_step: 5 time_step: 3 AckermannConstraints: min_turning_r: 0.2 critics: [ "ConstraintCritic", #0 "CostCritic", #1 "GoalCritic", #2 "GoalAngleCritic", #3 "TerminalGoalCritic", #9 "PathAlignCritic", #4 "CurvatureOnlyPathAlignCritic", #5 "PathFollowCritic", #6 "PathAngleCritic", #7 "PreferForwardCritic", #8 ] ConstraintCritic: enabled: true cost_power: 1 cost_weight: 4.0 GoalCritic: # [DISABLED] Superseded by TerminalGoalCritic, which now owns the whole band # below 1.4 m. Both are UPPER gates on local_path_length, so leaving this on # meant two position critics running together below 1.2 m. Three problems with # that, none of them a disagreement about the optimum -- measured, the two rank # rollouts identically in every case: # 1. Double counting. Each critic gets balanced to std/T 0.5-2.0 on its own, # so two overlapping position terms put the COMBINED authority near 4 T. # 2. It dilutes the anisotropy. This critic is isotropic (euclidean), the # terminal one weights lateral 1.5x because a diff drive cannot fix lateral # error. Measured lateral/longitudinal penalty ratio: 1.87 terminal alone, # 1.16 this one alone, 1.65 for the sum -- pulled back toward isotropic. # 3. A second relay. Two gates on a jittery local_path_length means two step # changes in total cost during the terminal approach, which is the same # gating pattern blamed for the weave everywhere else in this file. # # What is given up: this critic scores the MEAN distance over the horizon, so it # rewards arriving EARLY, which the terminal critic is indifferent to (measured # flat, 0.045 vs 0.050, across every cruise speed that still parks). The cost is # well under a second per goal over the last 1.4 m, and creeping is still # penalised by the terminal critic (2.12 T worse than parking). # # To revert: set enabled back to true and drop TerminalGoalCritic's # threshold_to_consider to 1.2 -- but then tune the two AS A PAIR on their # combined costs_std, never separately. enabled: false cost_power: 1 cost_weight: 5.0 threshold_to_consider: 1.4 GoalAngleCritic: # [NOTE] This critic may be spending its weight on the wrong axis. Two facts: # # 1. This robot is NONHOLONOMIC -- motion_model is diff_drive and vy_max is 0.0, # so DiffDriveMotionModel::isHolonomic() is false. It has two controls, v and # w, and cannot translate sideways. The only ways it can change yaw are to # curve (v != 0, w != 0), which moves x and y with it, or to stop and spin # (v = 0), which surrenders all forward progress -- and under payload the # wheel scrub of a spin drags the body anyway. There is no way for it to fix # yaw for free. On an omni platform none of this would apply. # # 2. rotate_to_goal_heading is true, so the shim redoes terminal yaw regardless, # and computeRotateToHeadingCommand() only ever sets twist.angular.z -- it # never commands vx. Position error at that handover is therefore FROZEN. # # Together: this critic is active in the last 0.5 m while the robot is still # approaching, so the cheapest way for a rollout to reduce yaw error here is to # curve -- which displaces the very terminal position the goal tolerance is # measured on. It buys yaw, which the shim will redo anyway, with position, # which the shim cannot fix. TerminalGoalCritic carries a small yaw term of its # own (yaw_weight 0.15) for the one job that does matter: keeping the shim's # spin short, since a long spin drags xy through slip. # # With rotate_to_goal_heading enabled, the shim redoes terminal yaw # anyway, and on a diff drive robot this critic buys that yaw with POSITION error # -- which is the one thing the shim cannot fix, because it commands wz only. # TerminalGoalCritic carries a small yaw term of its own for the same job. # Once TerminalGoalCritic is tuned, try dropping this to ~1.5 or disabling it # and compare goal xy p95; do NOT change both in the same run. enabled: true cost_power: 1 cost_weight: 3.0 threshold_to_consider: 0.5 symmetric_yaw_tolerance: true TerminalGoalCritic: # Parks the robot ON the goal over the last metre. # # [!] cost_weight below is a STARTING POINT, not a tuned value. Balance it the # way every other critic here was: read costs_std off critics_stats, divide by # cost_weight to get the raw-unit spread, and pick the weight that lands # std/T = cost_weight * raw_spread / 0.4608 # in the 0.5-2.0 band. This critic only scores the last 0.25 s of each rollout, # so its raw spread will NOT resemble GoalCritic's -- measure, do not scale. # # This fork also publishes effective sample size on critics_stats. Watch it # during the terminal approach: collapsing toward 1 means the softmax has # narrowed onto a single sampled rollout and the command is a fresh random draw # each cycle (jitter at the goal); sitting near batch_size means nothing is # steering. That is the fastest signal that this weight is too high or too low. enabled: true cost_power: 1 cost_weight: 8.0 # 1.4 m is where PathFollowCritic gates OFF, so this is a single clean handover: # one critic owns longitudinal progress above it, one owns parking below it, and # exactly one gate fires. Do not lower this while GoalCritic is disabled -- it # would leave a band with no goal-seeking term at all. threshold_to_consider: 1.4 # 5 x model_dt 0.05 = the last 0.25 s of the rollout. terminal_window: 5 # Lateral > longitudinal: a diff drive drives longitudinal error out on the # spot, but lateral error needs a manoeuvre it has no room for this close in. lateral_weight: 1.5 longitudinal_weight: 1.0 # Deliberately small -- the rotation shim owns terminal yaw. This only keeps # the shim's in-place spin short, because that spin drags xy through slip. yaw_weight: 0.15 # What separates "park here" from "pass through here". Units are m/s and rad/s # against metres of position error; at the near-goal caps (vx 0.20, wz 0.25) # they are worth about 0.10 m and 0.05 m of equivalent position error. stop_vx_weight: 0.15 stop_wz_weight: 0.2 # Match BidirectionalGoalChecker and the shim's selectBidirectionalHeading. symmetric_yaw_tolerance: true PreferForwardCritic: enabled: false cost_power: 1 cost_weight: 5.0 threshold_to_consider: 0.5 CostCritic: enabled: true cost_power: 1 cost_weight: 3.0 near_collision_cost: 253 critical_cost: 300.0 consider_footprint: false collision_cost: 1000000.0 near_goal_distance: 1.0 trajectory_point_step: 2 PathAlignCritic: ## Default value # cost_weight: 14.0 # max_path_occupancy_ratio: 0.05 # threshold_to_consider: 0.5 # offset_from_furthest: 20 enabled: true cost_power: 1 # Balance critics on the SPREAD of their cost across the batch (critics_stats # costs_std), not costs_sum: MPPI softmaxes over (cost - min), so a term that shifts # every trajectory equally has no effect. Target std ~= 0.5-2.0 x temperature (0.4608). # # Measured raw-unit spread (costs_std / cost_weight) at 1.1 m/s: 0.0382. So # std/T = cost_weight * 0.0382 / 0.4608 # 24.7832 -> 2.05 (tracked well, but could stall at a sharp corner) # 10.0 -> 0.83 (overshot: path_deviation p95 rose to 0.53 m) # 20.0 -> 1.66 cost_weight: 25.0 #### 10.0, 24.7832, 13.0 max_path_occupancy_ratio: 0.10 trajectory_point_step: 4 threshold_to_consider: 0.15 #### 0.5 # NOTE: this is a GATE, not an offset: the critic returns early when # furthest_reached_path_point < offset_from_furthest. That index was measured # oscillating around 20 at 10 Hz, so at 20 a weight-24.8 term switched fully on and # off every control cycle. Keep it well below the usual value of furthest. offset_from_furthest: 4 #### 20 use_path_orientations: false CurvatureOnlyPathAlignCritic: enabled: true cost_power: 1 # Measured raw-unit spread 0.0339, so std/T = cost_weight * 0.0339 / 0.4608. # At 3.0 this sat at 0.22, close to the ~0.2 floor below which a critic stops # influencing the result at all. 8.0 -> 0.59. cost_weight: 8.0 #### 3.0, 9.0385 max_path_occupancy_ratio: 0.07 trajectory_point_step: 2 threshold_to_consider: 0.5 offset_from_furthest: 8 use_path_orientations: false curvature_gain: 2.0 max_curvature: 1.5 max_curvature_extra: 2.0 curvature_influence_distance: 2.4851 PathFollowCritic: ## Default value # cost_weight: 5.0 # offset_from_furthest: 5 enabled: true cost_power: 1 # The only critic that rewards making progress along the path, so it must stay strong # enough that standing still is never the cheapest option. But it only constrains the # trajectory's END POINT -- it says nothing about the shape in between, so letting it # dominate makes the trajectory cut corners and drift off the path. # # Measured raw-unit spread 0.2069 (5.4x PathAlignCritic's -- which is why its weight # is numerically the smallest of the three while still being influential): # std/T = cost_weight * 0.2069 / 0.4608 # 3.9714 -> 1.78 8.0 -> 3.59 (dominated everything, poor tracking) 4.5 -> 2.02 cost_weight: 4.5 #### 8.0, 3.9714 offset_from_furthest: 7 threshold_to_consider: 1.4 PathAngleCritic: ## Default value # offset_from_furthest: 4 # threshold_to_consider: 0.5 # max_angle_to_furthest: 1.0 # The only critic that reads trajectory yaws. Without it, at v ~ 0 every remaining # critic gives an identical trajectory for w = -1.0 and w = +1.0, so the cost is flat # in angular velocity and the robot has no gradient to rotate itself out of a stall. enabled: true cost_power: 1 # Corrective, not tracking -- keep low. Its cost is an angle in radians and the batch # terminal-yaw spread is ~0.3-0.5 rad, so 2.0 already lands at std/T ~1.3-2.2. # Do NOT raise above ~3: at 5 it saturates the softmax and drives the oscillation. cost_weight: 2.0 # The gate below is evaluated once at the robot's CURRENT pose, so this critic is # all-on or all-off for the whole batch and behaves as a relay with no hysteresis -- # the oscillation source. Pushing the target point further out (12 -> 25 indices, so # ~0.6 m -> ~1.25 m past furthest_reached_path_point at 0.05 m path density) makes the # bearing far less sensitive to that index's jitter, which is what chatters the relay. offset_from_furthest: 25 #### 12 threshold_to_consider: 0.6 #### 0.5 # Do NOT widen to quiet the oscillation: at the measured stall the robot-to-target # angle was 0.68 rad, so at 0.6 the critic fires and at 0.8 it would never engage. max_angle_to_furthest: 0.6 mode: 1 #### 2: consider feasible path orientation forward_preference: false ## this param seem not working anymore # TwirlingCritic: # enabled: true # twirling_cost_power: 1 # twirling_cost_weight: 10.0 ## ============================================ ## Docking Related (FollowPathWithBaseController) ## ## ============================================ BaseGoal: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.001 yaw_goal_tolerance: 0.001 stateful: True FollowPathWithBaseController: plugin: "nav2_hmc_controller/Nav2ControllerROS" # GA_MPC Base v_max: 0.3 # 0.8 1.0 w_max: 0.4 ##### 0.24 v_min: 0.1 # 0.18 0.3 v_acc: 0.1 # 2.05 # 0.15 # 0.085 # 0.06 # 0.09 0.4 w_acc: 0.5 # 2.05 # 0.15 # 0.085 # 0.06 # 0.09 0.4 ##### 0.025 v_deacc: 0.35 projection_time_max: 1.2 learning_mlp: False fixed_reverse: True # False # fixed_reverse_value: True # GA_MPC Monte Carlo Sampling time_steps: 150 # 150 model_dt: 0.02 batch_size: 2000 # 2000 iteration_count: 1 ax_max: 3.0 ax_min: -3.0 vx_max: 1.0 # 1.0 vx_min: 0.0 # -1.0 vx_std: 0.1 # 0.45 0.05 0.1 wz_std: 0.1 # 0.18 0.05 0.1 wz_max: 2.0 vx_acc_limit: 1.0 ##### lift up 0.5 ##### down 1.0 vx_decel_limit: 0.7 ##### lift up 0.3 ##### down 0.7 wz_acc_limit: 1.8 ##### lift up 0.9 ##### down 1.8 wz_decel_limit: 1.8 ##### lift up 0.9 ##### down 1.8 acceleration_limited_sampling: true temperature: 0.6 # 0.2 0.4 gamma: 0.015 # 0.015 # This controller embeds the nav2_hmc_mppi_controller optimizer, whose # setMotionModel() takes a fixed string ("DiffDrive" / "Omni" / # "Ackermann") and throws on anything else. The pluginlib namespace form # ("diff_drive" + a diff_drive.plugin block) belongs to the *upstream* # nav2_mppi_controller only, and was previously set here by mistake. motion_model: "DiffDrive" mode : "GA-MPC" # SMOOTH, SMOOTH-PID, GA-MPC, PID, MPPI, visualize: true reset_period: 1.0 # (only in Humble) regenerate_noises: false adaptive_exploration: false # GA_MPC Kinematics Based Geometry fixed_smooth: false # mode must be smooth_mppi transform_tolerance: 0.1 motion_target_dist: 50.0 #4.0 initial_rotation: true initial_rotation_min_angle: 6.28 lateral_constraint_dist: 0.5 final_rotation: true k_phi: 6.0 # 8.0 6.0 9.0 6.0 7.0 6.0 8.0 6.0 k_delta: 2.0 # 2.0 6.0 4.0 1.0 2.0 beta: 0.2 lambda: 1.5 v_linear_min: 0.1 # 0.05 v_linear_max: 1.0 # 0.6 0.8 0.6 v_angular_max: 2.0 # 2.0 1.6 slowdown_radius: 0.1 # 3.0 4.0 qr_marker_width: 0.1 # 0.1 qr_marker_length: 1.4 # 1.5 static_wall: 2.5 ##### 2.3 2.2 # GA_MPC PID pid_vel: 0.1 # 0.35 0.5 0.4 0.3 0.2 pid_thres: 0.01 angular_heading_dist: 0.3 lookahead_nav_factor: 2.0 lookahead_dock_factor: 3.0 # 2.0 goal_dist_thres: 0.00 kp_angular: 2.4 # 0.1 0.05 0.8 0.1 3.6 ki_angular: 0.3 # 0.3 kd_angular: 0.1 # 1.4 0.1 kp_lookahead: 2.4 # 1.0 0.8 1.2 1.4 2.0 2.8 5.0 1.4 4.2 1.4 2.4 ki_lookahead: 0.3 # 0.3 2.3 kd_lookahead: 2.1 # 1.4 4.0 2.4 2.8 1.0 0.1 2.1 # GA_MPC Critics # These resolve through the nav2_hmc_mppi_controller critic ClassLoader, # which is where CurvatureAwareCritic / PayloadJerkCritic / # GoalTargetVelocityCritic live. They do NOT exist in the upstream # nav2_mppi_controller shipped by hmgics_navigation2. critics: ["PathAlignCritic", "CurvatureAwareCritic", "PathAngleCritic"] ##### GoalTargetVelocityCritic PayloadJerkCritic: enabled: true cost_weight: 0.1 cost_power: 1 weight_v: 0.8 weight_y: 0.2 weight_w: 0.2 robot_mass: 1.0 # amr weight : 100kg robot_inertia: 1.0 # 25 * 0.02 = 0.5 sec peak_window_steps: 25 # secondary critic cap max_payload_cost: 1.0 smooth_v_threshold: 0.02 smooth_w_threshold: 0.02 force_rate_x_ref: 1.0 force_rate_y_ref: 1.0 torque_rate_ref: 1.0 model_dt: 0.02 use_dt_normalization: false use_squared_cost: false CurvatureAwareCritic: enabled: true cost_weight: 2.0 # 1.0 cost_power: 1 v_max: 0.7 ##### lift up 0.5 ##### lift down 0.7 ##### 0.3 kappa_threshold: 0.0 preview_distance: 2.0 mean_cost_scale: 180.0 # 0.25 = 현재 kappa_future_raw 25%, 이전 smoothed 75% kappa_future_smoothing_alpha: 0.5 PathAlignCritic: enabled: true cost_power: 1 cost_weight: 100.0 trajectory_point_step: 4 # 4 ##### 10 threshold_to_consider: 0.05 ###### 0.05 offset_from_furthest: 1 PathAngleCritic: enabled: true cost_power: 1 cost_weight: 6.0 # 18.0 # ↓ 회전비중 조정 (경로 정렬은 유지) offset_from_furthest: 4 # 4 threshold_to_consider: 0.05 # 0.18 0.1 ##### 0.08 max_angle_to_furthest: 0.05 # 0.18 0.1 ##### 0.08 forward_preference: true GoalCritic: enabled: true cost_power: 1 cost_weight: 5.0 threshold_to_consider: 2.0 GoalTargetVelocityCritic: enabled: true cost_weight: 1.0 cost_power: 1 # 1.2m 안쪽에서 유지할 목표 속도 target_v: 0.2 # 1.2m부터 goal까지는 0.1 유지 slow_radius: 1.2 # 1.2 # 1.2m 바깥쪽에서는 이 거리부터 overspeed cost가 점점 생김 outer_radius: 4.0 # 이 거리 밖에서는 critic 비활성 goal_active_radius: 4.0 # 0.0이면 horizon 전체 평가 preview_distance: 0.0 # 현재 raw cost가 작으면 20~50까지 올려도 됨 mean_cost_scale: 35.0 max_velocity_error: 1.0 use_squared_error: false local_costmap: local_costmap: ros__parameters: update_frequency: 10.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link map_topic: /map_nav rolling_window: true width: 15 height: 15 resolution: 0.05 # [from lidar calib] # footprint: "[[0.594907, -0.440983], [0.594907, 0.359017], [-0.545093, 0.359017], [-0.545093, -0.440983]]" # [from CAD] footprint: "[[0.572, -0.38], [0.572, 0.38], [-0.572, 0.38], [-0.572, -0.38]]" footprint_padding: 0.25 # plugins: ["obstacle_layer", "nonpersisting_obstacle_layer", "inflation_layer"] plugins: ["obstacle_layer", "inflation_layer"] filters: ["keepout_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" # enabled: KEEPOUT_ZONE_ENABLED enabled: False filter_info_topic: "keepout_costmap_filter_info" override_lethal_cost: True lethal_override_cost: 200 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 5.0 #### 3.0 inflation_radius: 1.7 #### 1.0 [no payload] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: true observation_sources: scan scan: # topic: /scan topic: /scan_filtered sensor_frame: "base_link" inf_is_valid: True max_obstacle_height: 2.0 clearing: true marking: true data_type: "LaserScan" obstacle_max_range: 8.0 obstacle_min_range: 0.0 raytrace_max_range: 10.0 # increase this => trace longer range for local cost map clearing raytrace_min_range: 0.0 observation_persistence: 0.2 # keep obstacle for 0.2 sec if not seen nonpersisting_obstacle_layer: plugin: "nav2_costmap_2d/NonPersistentVoxelLayer" enabled: true track_unknown_space: true max_obstacle_height: 3.0 unknown_threshold: 15 mark_threshold: 2 combination_method: 1 obstacle_range: 2.0 origin_z: 0. z_resolution: 0.05 z_voxels: 16 publish_voxel_map: false observation_sources: depth depth: data_type: PointCloud2 topic: /amr_vision/residual_cell_points marking: true min_obstacle_height: 0.12 max_obstacle_height: 2.8 static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True always_send_full_costmap: True service_introspection_mode: "disabled" global_costmap: global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map robot_base_frame: base_link # footprint: "[[0.594907, -0.440983], [0.594907, 0.359017], [-0.545093, 0.359017], [-0.545093, -0.440983]]" footprint: "[[0.572, -0.38], [0.572, 0.38], [-0.572, 0.38], [-0.572, -0.38]]" footprint_padding: 0.25 resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] filters: ["keepout_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" # enabled: KEEPOUT_ZONE_ENABLED enabled: False filter_info_topic: "keepout_costmap_filter_info" override_lethal_cost: True lethal_override_cost: 200 speed_filter: plugin: "nav2_costmap_2d::SpeedFilter" enabled: SPEED_ZONE_ENABLED filter_info_topic: "speed_costmap_filter_info" speed_limit_topic: "speed_limit" obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan scan: # topic: /scan topic: /scan_filtered inf_is_valid: True # [16July] default false max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" raytrace_max_range: 9.0 # [16July] 3.0 raytrace_min_range: 0.0 obstacle_max_range: 8.0 # [16July] 2.5 obstacle_min_range: 0.0 static_layer: topic: "/map_nav" plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 5.0 #### 3.0 inflation_radius: 1.7 #### 1.0 [no payload] always_send_full_costmap: True service_introspection_mode: "disabled" keepout_filter_mask_server: ros__parameters: topic_name: "keepout_filter_mask" # yaml_filename: "" keepout_costmap_filter_info_server: ros__parameters: type: 0 filter_info_topic: "keepout_costmap_filter_info" mask_topic: "keepout_filter_mask" base: 0.0 multiplier: 1.0 speed_filter_mask_server: ros__parameters: topic_name: "speed_filter_mask" # yaml_filename: "" speed_costmap_filter_info_server: ros__parameters: type: 1 filter_info_topic: "speed_costmap_filter_info" mask_topic: "speed_filter_mask" base: 100.0 multiplier: -1.0 planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased", "AstarGridBased", "GridBasedArc"] costmap_update_timeout: 1.0 service_introspection_mode: "disabled" AstarGridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.05 use_astar: true allow_unknown: true GridBased: plugin: "nav2_smac_planner::SmacPlannerHybrid" 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.10 # 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: "REEDS_SHEPP" # 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 minimum_turning_radius: 0.80 reverse_penalty: 1.2 # Penalty to apply if motion is reversing, must be => 1 # change_penalty: 0.2 # Penalty to apply if motion is changing directions (L to R), must be >= 0 change_penalty: 0.4 # non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 non_straight_penalty: 1.5 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_heuristic_lookup_table: true # Persist the dubin/reeds-sheep distance window to disk so restarts skip the precompute. Not tied to the map; invalidated by costmap resolution, minimum_turning_radius, lookup_table_size, angle_quantization_bins or motion_model_for_search. heuristic_cache_dir: "/home/hyundai-motor/standard_amr_ws" # Where to keep that cache. Empty -> $ROS_HOME/nav2_smac_planner (i.e. ~/.ros/nav2_smac_planner). 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 coarse_search_resolution: 4 # Number of bins to skip when doing a coarse search for the path. Only used for all_direction goal heading mode. goal_heading_mode: "DEFAULT" # DEFAULT, BIDIRECTIONAL, 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 ## ============================================ ## Docking Related (GridBasedArc) ## ## ============================================ GridBasedArc: plugin: "nav2_hmc_planner/Nav2PlannerROS" step_arc_length: 0.025 step_str_length: 0.025 const_path_length: 2.0 arc_radius: 0.1 # 0.3 # 1.5 sc_arc_goal: 0.1 # 0.5 sc_str_goal: 0.1 # 0.5 pr_goal: 0.5 # 0.1 goal_checker: 0.5 lift_stop_sec: 4.0 extension_dist: 0.3 use_reverse: true dock_registry_path: "" smoother_server: ros__parameters: use_sim_time: False smoother_plugins: ["simple_smoother", "route_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" tolerance: 1.0e-10 do_refinement: True max_its: 1000 w_data: 0.3 #path data weight w_smooth: 0.2 #path를 smooth할 때 weight # route_smoother: # plugin: "nav2_smoother::SimpleSmoother" # tolerance: 1.0e-10 # max_its: 1000 # refinement_num: 5 # # False for Route Server or others where smoothing out sharp corners is OK # enforce_path_inversion: False # do_refinement: True route_smoother: # [NOTE] CornerFilletSmoother rounds every corner of the final concatenated # path to smoothing_radius, independent of the route window. This is why # route_server smooth_corners is false: corner rounding is owned here (always # runs on SmoothPath) instead of in the route server (only fillets junctions # interior to the sliding base window, so corners flicker as the robot turns). plugin: "nav2_smoother::CornerFilletSmoother" smoothing_radius: 2.0 # match route_server smoothing_radius density: 0.05 # arc/segment sample spacing (m) simplify_tolerance: 0.10 # RDP epsilon; raise if the A* first-mile staircase makes fake corners min_turn_angle: 0.35 # skip fillet below ~20 deg turns 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" acceleration_limit: 2.5 deceleration_limit: -2.5 minimum_speed: 0.10 drive_on_heading: plugin: "nav2_behaviors::DriveOnHeading" acceleration_limit: 2.5 deceleration_limit: -2.5 minimum_speed: 0.10 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 route_server: ros__parameters: debug_traversability_rejections: true # The graph_filepath 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 "graph" value in the launch file(s). # file & provide full path to map below. If graph config or launch default is provided, it is used # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson boundary_radius_to_achieve_node: 1.0 radius_to_achieve_node: 2.0 smoothing_radius: 2.0 ## 1.5 smooth_corners: false # operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] operations: ["AdjustSpeedLimit"] ReroutingService: plugin: "nav2_route::ReroutingService" AdjustSpeedLimit: plugin: "nav2_route::AdjustSpeedLimit" CollisionMonitor: plugin: "nav2_route::CollisionMonitor" max_collision_dist: 3.0 # edge_cost_functions: ["DistanceScorer", "CostmapScorer"] edge_cost_functions: ["DistanceScorer"] DistanceScorer: plugin: "nav2_route::DistanceScorer" CostmapScorer: plugin: "nav2_route::CostmapScorer" velocity_smoother: ros__parameters: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" max_velocity: [1.0, 0.0, 1.2] min_velocity: [-1.0, 0.0, -1.2] max_accel: [3.0, 0.0, 3.5] max_decel: [-3.0, 0.0, -3.5] 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_link" 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.5 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: ["PathCorridorStop", "FootprintApproach"] PathCorridorStop: enabled: False type: "path_polygon" action_type: "stop" path_topic: "/controller_server/transformed_global_plan" corridor_width: 2.0 lookahead_distance: 3.5 path_timeout: 1.0 centerline_sample_spacing: 0.1 max_polygon_points: 100 min_path_length: 0.2 near_goal_distance: 2.0 # disable corridor stop within 2.0 m of goal; 0.0 = off (must be < prune_distance) min_points: 3 visualize: True polygon_pub_topic: "path_corridor_stop_zone" # sources_names: ["scan","pointcloud"] sources_names: ["scan"] FootprintApproach: enabled: False type: "polygon" action_type: "approach" footprint_topic: "local_costmap/published_footprint" time_before_collision: 1.8 ## original: 1.2 simulation_time_step: 0.1 min_points: 6 visualize: True # observation_sources: ["scan","pointcloud"] observation_sources: ["scan"] scan: type: "scan" # topic: "/scan" topic: /scan_filtered min_height: 0.15 max_height: 2.0 enabled: True pointcloud: type: "pointcloud" topic: "/amr_vision/residual_cell_points" transport_type: "raw" min_height: 0.12 max_height: 2.0 min_range: 0.2 enabled: True