add new file nav_c_api2 and function
This commit is contained in:
@@ -8,4 +8,8 @@ voxel_layer:
|
||||
unknown_threshold: 15.0
|
||||
mark_threshold: 0
|
||||
combination_method: 1
|
||||
|
||||
frustum_clearing_enabled: true
|
||||
frustum_clearing_pixel_step: 8
|
||||
frustum_min_range: 0.20
|
||||
frustum_max_range: 3.0
|
||||
frustum_depth_camera_topic: /camera/depth/data
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 1.0
|
||||
performance_metrics_enabled: true
|
||||
performance_metrics_period: 5.0
|
||||
obstacle_range: 3.0
|
||||
#mark_threshold: 1
|
||||
publish_voxel_map: true
|
||||
@@ -19,7 +21,7 @@ virtual_walls_map:
|
||||
lethal_cost_threshold: 100
|
||||
|
||||
obstacles:
|
||||
observation_sources: b_scan_marking b_scan_clearing pc_marking pc_clearing
|
||||
observation_sources: b_scan pc_marking pc_clearing pc_r_marking pc_r_clearing
|
||||
# f_scan_marking: f_scan_marking f_scan_clearing
|
||||
# topic: /f_scan
|
||||
# data_type: LaserScan
|
||||
@@ -36,20 +38,13 @@ obstacles:
|
||||
# inf_is_valid: true
|
||||
# min_obstacle_height: 0.0
|
||||
# max_obstacle_height: 0.25
|
||||
b_scan_marking:
|
||||
topic: /b_scan
|
||||
data_type: LaserScan
|
||||
clearing: false
|
||||
marking: true
|
||||
inf_is_valid: true
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 0.25
|
||||
b_scan_clearing:
|
||||
b_scan:
|
||||
topic: /b_scan
|
||||
data_type: LaserScan
|
||||
clearing: true
|
||||
marking: false
|
||||
marking: true
|
||||
inf_is_valid: true
|
||||
frustum_clearing_enabled: false
|
||||
min_obstacle_height: 0.0
|
||||
max_obstacle_height: 0.25
|
||||
|
||||
@@ -59,6 +54,7 @@ obstacles:
|
||||
clearing: false
|
||||
marking: true
|
||||
inf_is_valid: false
|
||||
frustum_clearing_enabled: false
|
||||
observation_persistence: 0.0
|
||||
expected_update_rate: 0.5
|
||||
obstacle_range: 2.5
|
||||
@@ -67,14 +63,38 @@ obstacles:
|
||||
max_obstacle_height: 1.0
|
||||
|
||||
pc_clearing:
|
||||
topic: /camera/depth/points_clear
|
||||
topic: /camera/depth/data
|
||||
data_type: DepthCameraData
|
||||
clearing: false
|
||||
marking: true
|
||||
frustum_clearing_enabled: true
|
||||
frustum_clearing_pixel_step: 8
|
||||
frustum_min_range: 0.20
|
||||
frustum_max_range: 3.5
|
||||
|
||||
pc_r_marking:
|
||||
topic: /camera_right/depth/points_proc
|
||||
data_type: PointCloud2
|
||||
clearing: true
|
||||
marking: false
|
||||
clearing: false
|
||||
marking: true
|
||||
inf_is_valid: false
|
||||
frustum_clearing_enabled: false
|
||||
observation_persistence: 0.0
|
||||
expected_update_rate: 0.5
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
min_obstacle_height: -0.5
|
||||
max_obstacle_height: 5.0
|
||||
min_obstacle_height: 0.1
|
||||
max_obstacle_height: 1.0
|
||||
|
||||
pc_r_clearing:
|
||||
topic: /camera_right/depth/data
|
||||
data_type: DepthCameraData
|
||||
clearing: false
|
||||
marking: true
|
||||
frustum_clearing_enabled: true
|
||||
frustum_clearing_pixel_step: 8
|
||||
frustum_min_range: 0.20
|
||||
frustum_max_range: 3.5
|
||||
|
||||
# Depth camera clearing is handled by VoxelLayer frustum clearing from
|
||||
# /camera/depth/image_raw + /camera/depth/camera_info. /camera_right/depth/data
|
||||
|
||||
@@ -1,32 +1,17 @@
|
||||
LocalPlannerAdapter:
|
||||
library_path: liblocal_planner_adapter
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.03
|
||||
min_approach_linear_velocity: 0.06
|
||||
|
||||
HybridLocalPlanner:
|
||||
# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
|
||||
# HybridLocalPlanner:
|
||||
library_path: libhybrid_local_planner
|
||||
# Robot
|
||||
robot_max_v_ac: 0.4
|
||||
robot_max_w_ac: 0.6
|
||||
robot_max_v_pt: 1.0
|
||||
robot_max_w_pt: 0.6
|
||||
robot_min_v: -0.5
|
||||
robot_min_w: 0.05
|
||||
robot_max_v_backwards_pt: -0.25
|
||||
acc_lim_x: 1.0
|
||||
max_forward_velocity: 1.0
|
||||
max_reverse_velocity: 0.25 # [m/s], positive magnitude
|
||||
avoidance_max_velocity: 0.4
|
||||
max_angular_velocity: 0.6
|
||||
min_in_place_angular_velocity: 0.3
|
||||
acc_lim_x: 0.1
|
||||
decel_lim_x: 0.5
|
||||
acc_lim_theta: 2.0
|
||||
min_turn_radius: 0.0
|
||||
turn_around_priority: true
|
||||
stop_dist: 0.5
|
||||
dec_dist: 1.0
|
||||
max_acceleration: 0.1
|
||||
max_deceleration: 0.5
|
||||
max_d_yawrate: 3.2
|
||||
max_in_place_yawrate: 0.6
|
||||
min_in_place_yawrate: 0.3
|
||||
robot_radius: 0.1
|
||||
footprint_padding: 0.08
|
||||
use_footprint: true
|
||||
@@ -39,26 +24,32 @@ HybridLocalPlanner:
|
||||
max_global_plan_lookahead_dist: 3.0
|
||||
global_plan_viapoint_sep: 0.5
|
||||
global_plan_goal_sep: 0.05
|
||||
global_plan_prune_distance: 0.0
|
||||
predict_time: 3.0
|
||||
sim_period: 0.1
|
||||
sim_time_samples: 10
|
||||
sim_direction: M_PI / 2.0
|
||||
|
||||
# Optimization
|
||||
w_vel: 0.8
|
||||
w_omega: 2.5
|
||||
obs_cost_gain: 1.3
|
||||
path_cost_gain: 2.0
|
||||
path_cost_gain: 0.5
|
||||
to_goal_cost_gain: 0.8
|
||||
speed_cost_gain: 0.8
|
||||
speed_cost_gain: 0.5
|
||||
|
||||
# Obstacles
|
||||
obs_range: 2.5
|
||||
|
||||
#GeneralSetting
|
||||
segment_transition_threshold: 0.01
|
||||
calibration_factor: 1.5
|
||||
segment_transition_threshold: 0.03 # [m], also used as DWA stop distance for intermediate segments
|
||||
rotation_segment_yaw_threshold: 0.05 # [rad], ignore tiny yaw noise at duplicate positions
|
||||
direction_change_hysteresis: 0.15 # normalized dot product
|
||||
use_obstacle_avoidance: true
|
||||
slow_velocity_th: 0.1
|
||||
turn_direction_th: 0.1
|
||||
turn_direction_th: 0.1
|
||||
avoidance_clear_hold_time: 0.5 # [s]
|
||||
rejoin_blend_time: 0.8 # [s]
|
||||
rejoin_max_velocity: 0.25 # [m/s]
|
||||
rejoin_heading_tolerance: 0.35 # [rad]
|
||||
rejoin_path_tolerance: 0.20 # [m]
|
||||
pp_max_omega_correction: 0.15 # [rad/s]
|
||||
rejoin_acc_lim_theta: 1.0 # [rad/s^2]
|
||||
rejoin_lookahead_distance: 0.8 # [m]
|
||||
|
||||
42
config/mission_adapters_params.yaml
Normal file
42
config/mission_adapters_params.yaml
Normal file
@@ -0,0 +1,42 @@
|
||||
# Tham số RUNTIME của mission layer (gói `mission_adapters`).
|
||||
#
|
||||
# Đây là bản đang có hiệu lực. Bản trong `Test/mission_adapters/test/config/` chỉ phục vụ test và
|
||||
# chỉ được đọc khi chạy kèm PNKX_NAV_CORE_CONFIG_DIR trỏ vào đúng thư mục đó.
|
||||
|
||||
mission_adapters:
|
||||
# Nguồn mission. Thêm một loại nguồn mới = thêm một entry ở đây + một plugin .so, không phải sửa
|
||||
# code của gói.
|
||||
# name: tên instance, cũng là namespace param riêng của nó
|
||||
# type: tên symbol export bằng BOOST_DLL_ALIAS, phải có khoá library_path tương ứng bên dưới
|
||||
mission_sources:
|
||||
- {name: goal_src, type: GoalSourceAdapter}
|
||||
- {name: vda5050_src, type: VDA5050SourceAdapter}
|
||||
|
||||
# Trần thời gian cho MỘT chặng (navigation + action), tính từ lúc chặng được giao. [s]
|
||||
# 0 = tắt. Quá hạn thì chặng bị đánh dấu thất bại VÀ navigation được bảo dừng — lưới cuối cho
|
||||
# trường hợp navigation không bao giờ báo kết quả về.
|
||||
mission_timeout: 0.0
|
||||
|
||||
# Một chặng thất bại thì xử lý phần còn lại của hàng đợi thế nào.
|
||||
# true = xoá sạch hàng đợi (mặc định, an toàn cho tuyến đường tuần tự kiểu VDA5050: không tới
|
||||
# được node n thì chạy tiếp chặng n+1 là cắt ngang đoạn chưa được cho phép đi)
|
||||
# false = chỉ bỏ chặng lỗi rồi chạy tiếp. Chỉ đặt false khi các mission trong hàng đợi ĐỘC LẬP
|
||||
# với nhau, không phải các chặng của cùng một tuyến.
|
||||
clear_queue_on_failure: true
|
||||
|
||||
# Param riêng của từng instance nguồn mission, đặt theo `name` ở trên.
|
||||
vda5050_src:
|
||||
# Frame gán cho goal/start sinh ra từ nodePosition của VDA5050.
|
||||
# Lưu ý: `mapId` trong VDA5050 là danh tính bản đồ, KHÔNG phải frame TF, nên không dùng làm frame.
|
||||
global_frame: map
|
||||
|
||||
# Bảng symbol -> thư viện cho Boost.DLL.
|
||||
#
|
||||
# Thiếu khoá library_path là nguyên nhân phổ biến nhất của lỗi "plugin build xong nhưng runtime báo
|
||||
# không tìm thấy". Tên không có đuôi .so được resolve qua PNKX_NAV_CORE_LIBRARY_PATH / devel/lib /
|
||||
# LD_LIBRARY_PATH — tức phải `source devel/setup.bash` trước khi chạy.
|
||||
GoalSourceAdapter:
|
||||
library_path: libmission_adapters_goal_source
|
||||
|
||||
VDA5050SourceAdapter:
|
||||
library_path: libmission_adapters_vda5050_source
|
||||
@@ -1,43 +1,18 @@
|
||||
# position_planner_name: PNKXLocalPlanner
|
||||
# docking_planner_name: PNKXDockingLocalPlanner
|
||||
# position_planner_name: StanleyLocalPlanner
|
||||
# docking_planner_name: StanleyDockingLocalPlanner
|
||||
position_planner_name: MPPILocalPlanner #HybridLocalPlanner MPPILocalPlanner
|
||||
docking_planner_name: PNKXDockingLocalPlanner
|
||||
position_planner_name: PriestLocalPlanner #HybridLocalPlanner MPPILocalPlanner PriestLocalPlanner PNKXLocalPlanner
|
||||
docking_planner_name: PNKXDockingLocalPlanner #StanleyDockingLocalPlanner PNKXDockingLocalPlanner
|
||||
go_straight_planner_name: PNKXGoStraightLocalPlanner
|
||||
rotate_planner_name: PNKXRotateLocalPlanner
|
||||
base_local_planner: LocalPlannerAdapter
|
||||
base_global_planner: CustomPlanner
|
||||
|
||||
# PNKXLocalPlanner:
|
||||
# base_local_planner: LocalPlannerAdapter
|
||||
# base_global_planner: SBPLLatticePlanner
|
||||
|
||||
# PNKXDockingLocalPlanner:
|
||||
# base_local_planner: LocalPlannerAdapter
|
||||
# base_global_planner: CustomPlanner
|
||||
|
||||
# StanleyLocalPlanner:
|
||||
# base_local_planner: LocalPlannerAdapter
|
||||
# base_global_planner: CustomPlanner
|
||||
|
||||
# StanleyDockingLocalPlanner:
|
||||
# base_local_planner: LocalPlannerAdapter
|
||||
# base_global_planner: CustomPlanner
|
||||
|
||||
# HybridLocalPlanner:
|
||||
# base_local_planner: LocalPlannerAdapter
|
||||
# base_global_planner: CustomPlanner
|
||||
|
||||
MPPILocalPlanner:
|
||||
PriestLocalPlanner:
|
||||
base_local_planner: LocalPlannerAdapter
|
||||
base_global_planner: SBPLLatticePlanner
|
||||
base_global_planner: CustomPlanner #CustomPlanner SBPLLatticePlanner
|
||||
|
||||
PNKXDockingLocalPlanner:
|
||||
base_local_planner: LocalPlannerAdapter
|
||||
base_global_planner: TwoPointsPlanner
|
||||
|
||||
|
||||
PNKXGoStraightLocalPlanner:
|
||||
base_local_planner: LocalPlannerAdapter
|
||||
base_global_planner: TwoPointsPlanner
|
||||
@@ -47,18 +22,28 @@ PNKXRotateLocalPlanner:
|
||||
base_global_planner: TwoPointsPlanner
|
||||
|
||||
### replanning
|
||||
controller_frequency: 30.0 # run controller at 15.0 Hz
|
||||
controller_frequency: 30.0 # run controller at 30.0 Hz
|
||||
controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
|
||||
planner_frequency: 0.0 # don't continually replan (only when controller failed)
|
||||
planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s...
|
||||
max_planning_retries: 0 # ... or after 10 attempts (whichever happens first)
|
||||
oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s
|
||||
oscillation_distance: 0.5
|
||||
### recovery behaviors
|
||||
recovery_behavior_enabled: true
|
||||
## recovery behaviors
|
||||
#
|
||||
# Recovery của move_base cũ đã dừng: bộ behavior gen-2 (tick-based) khai ở
|
||||
# `recovery_behaviors_params.yaml` và do `recovery_core::RecoveryRegistry` nạp, không phải khoá
|
||||
# `recovery_behaviors` ở đây.
|
||||
#
|
||||
# Danh sách gen-1 đã được gỡ hẳn thay vì để lại: các entry cũ trỏ tên alias `RotateRecovery` /
|
||||
# `ClearCostmapRecovery` vào file .so gen-2, trong khi loader ở đây import theo chữ ký gen-1
|
||||
# (`robot_nav_core::RecoveryBehavior`). Boost.DLL không kiểm kiểu qua ranh giới .so, nên hai bên
|
||||
# không bao giờ gặp nhau ở compile time và lỗi chỉ hiện ra lúc chạy. Giữ lại khoá cũng làm hai file
|
||||
# config tranh nhau cùng một tên alias.
|
||||
recovery_behavior_enabled: false
|
||||
recovery_behaviors: [
|
||||
{name: aggressive_reset, type: ClearCostmapRecovery},
|
||||
{name: conservative_reset, type: ClearCostmapRecovery}
|
||||
{name: conservative_reset, type: ClearCostmapRecovery},
|
||||
]
|
||||
|
||||
conservative_reset:
|
||||
|
||||
@@ -7,120 +7,92 @@ LocalPlannerAdapter:
|
||||
MPPILocalPlanner:
|
||||
library_path: libmppi_local_planner
|
||||
|
||||
# Robot limits [m/s, rad/s, m/s^2, rad/s^2]
|
||||
max_velocity: 0.5
|
||||
# Robot limits [m/s, rad/s]
|
||||
min_velocity: 0.0
|
||||
max_y_velocity: 0.0
|
||||
min_command_velocity: 0.05
|
||||
max_angular_velocity: 1.9
|
||||
min_angular_velocity: 0.05
|
||||
max_acceleration: 0.5
|
||||
max_angular_acceleration: 1.0
|
||||
max_vel_trans: 0.5
|
||||
# Cao hon reference_velocity mot khoang dem (headroom). Neu de sat ref thi
|
||||
# nua tren cua nhieu sampling bi clamp o max, keo van toc trung binh xuong
|
||||
# (do duoc: ref0.45/max0.50 -> chi dat 0.33; max0.65 -> dat 0.41). Robot chay
|
||||
# duoc >0.6 m/s nen dat 0.65.
|
||||
max_velocity: 0.65
|
||||
max_angular_velocity: 1.0
|
||||
robot_radius: 0.30
|
||||
obstacle_point_radius: 0.08
|
||||
footprint_padding: 0.05
|
||||
|
||||
# MPPI core
|
||||
# Horizon = model_dt * time_steps = 3.6 s ~ 1.8 m @0.5 m/s.
|
||||
# Truoc day 1.4 s (0.7 m) qua ngan: robot chi "thay" vat can khi da sat, khong con
|
||||
# cho de vong qua -> hoac ket hoac lang nhang. Do thuc te: 0.7m/1.2m khong toi dich;
|
||||
# 1.8m toi dich voi quang duong chi dai hon 6% so voi duong thang.
|
||||
# Khong tang dt qua 0.15: dt=0.20 lam dao dong tang gap 4 lan.
|
||||
model_dt: 0.2
|
||||
time_steps: 20
|
||||
batch_size: 220
|
||||
# So thread OpenMP. De mac dinh (= 16 core) thi tranh chap voi gazebo/rviz/perception
|
||||
# lam 1 chu ky solveMPPI cham tu 0.9 ms len ~25 ms.
|
||||
num_threads: 4
|
||||
param_lambda: 100.0
|
||||
param_alpha: 0.98
|
||||
param_exploration: 0.05
|
||||
vx_std: 0.2
|
||||
vy_std: 0.2
|
||||
wz_std: 0.4
|
||||
motion_model: 0
|
||||
min_turning_radius: 0.2
|
||||
wz_std: 0.25
|
||||
reference_velocity: 0.48
|
||||
|
||||
# Goal tolerance [m, rad]
|
||||
xy_goal_tolerance: 0.1
|
||||
yaw_goal_tolerance: 0.1
|
||||
stopped_xy_velocity: 0.03
|
||||
stopped_theta_velocity: 0.03
|
||||
|
||||
# MPPI horizon and global-plan handling
|
||||
model_dt: 0.05
|
||||
time_steps: 56
|
||||
batch_size: 1000
|
||||
iteration_count: 1
|
||||
temperature: 0.3
|
||||
gamma: 0.1
|
||||
retry_attempt_limit: 10
|
||||
regenerate_noises: false
|
||||
open_loop: false
|
||||
controller_frequency: 30.0
|
||||
prune_distance: 1.5
|
||||
max_robot_pose_search_dist: 0.0
|
||||
# Path and goal
|
||||
# Phai lon hon horizon (1.8 m), neu khong terminal goal_weight se keo robot ve
|
||||
# diem cat cua reference path va lam no giam toc moi chu ky.
|
||||
max_global_plan_lookahead_dist: 3.0
|
||||
global_plan_prune_distance: 0.2
|
||||
sim_period: 0.1
|
||||
xy_goal_tolerance: 0.10
|
||||
yaw_goal_tolerance: 0.10
|
||||
|
||||
# Costmap safety
|
||||
use_costmap_collision_check: true
|
||||
allow_unknown: true
|
||||
lethal_cost: 253
|
||||
inflation_score_distance: 0.3
|
||||
consider_footprint: true
|
||||
# Costmap and cost weights
|
||||
obstacle_range: 2.5
|
||||
obstacle_cost_threshold: 150
|
||||
obstacle_cell_step: 2
|
||||
max_obstacle_points: 180
|
||||
obstacle_influence_radius: 1.0
|
||||
# Cost vat can gio CONG DON tren tung cell (truoc day lay max), nen thang nay phai
|
||||
# nho hon nhieu. Giu 85 voi che do sum lam cost vot len ~180x85, ap dao moi thanh
|
||||
# phan khac -> dao dong manh va di vong xa 45%. Do thuc te tai T=24/dt=0.15:
|
||||
# weight=10 cho dao dong thap nhat va khoang ho 0.48 m (du 13 cm).
|
||||
obstacle_cost_use_sum: true
|
||||
obstacle_weight: 10.0
|
||||
collision_cost: 1000000.0
|
||||
progress_weight: 8.0
|
||||
terminal_progress_weight: 20.0
|
||||
goal_weight: 30.0
|
||||
# Phat v^2: gan 0 vi no truc tiep keo van toc xuong (phan truc giac voi robot
|
||||
# muon chay nhanh). Giu nho de van co chut smoothing.
|
||||
linear_control_weight: 0.2
|
||||
angular_control_weight: 0.2
|
||||
control_change_weight: 1.0
|
||||
# Giam toc khi lech huong: ha tu 0.8 xuong 0.3. Truoc day khi ne vat can
|
||||
# (yaw error lon) robot bo <0.1 m/s. 0.3 van giam toc vao cua nhung khong bo.
|
||||
yaw_error_slowdown: 0.1
|
||||
min_tracking_velocity: 0.08
|
||||
|
||||
# Critic defaults ported from the legacy MPPI dynamic_reconfigure files.
|
||||
constraint_enabled: true
|
||||
constraint_power: 1.0
|
||||
constraint_weight: 4.0
|
||||
# Phat hien ket: MPPI co the tra lenh hop le nhung ~0 mai o cuc tieu dia phuong.
|
||||
# Qua nguong nay controller bao that bai -> move_base PLANNING roi CLEARING.
|
||||
stuck_velocity_threshold: 0.03
|
||||
stuck_omega_threshold: 0.05
|
||||
stuck_patience_cycles: 40
|
||||
|
||||
cost_enabled: true
|
||||
cost_power: 1.0
|
||||
cost_weight: 3.81
|
||||
cost_critical_cost: 300.0
|
||||
cost_near_collision_cost: 253
|
||||
cost_collision_cost: 1000000.0
|
||||
cost_near_goal_distance: 1.0
|
||||
cost_trajectory_point_step: 2
|
||||
|
||||
goal_enabled: true
|
||||
goal_power: 1.0
|
||||
goal_weight: 5.0
|
||||
goal_threshold_to_consider: 1.4
|
||||
|
||||
goal_angle_enabled: true
|
||||
goal_angle_power: 1.0
|
||||
goal_angle_weight: 3.0
|
||||
goal_angle_threshold_to_consider: 0.5
|
||||
|
||||
obstacles_enabled: false
|
||||
obstacles_power: 1.0
|
||||
obstacles_weight: 0.0
|
||||
obstacles_repulsion_weight: 1.5
|
||||
obstacles_collision_cost: 10000.0
|
||||
obstacles_collision_margin_distance: 0.1
|
||||
obstacles_near_goal_distance: 0.5
|
||||
|
||||
path_align_enabled: true
|
||||
path_align_power: 1.0
|
||||
path_align_weight: 8.0
|
||||
path_align_max_path_occupancy_ratio: 0.07
|
||||
path_align_threshold_to_consider: 0.5
|
||||
path_align_offset_from_furthest: 20
|
||||
path_align_trajectory_point_step: 4
|
||||
path_align_use_path_orientations: false
|
||||
|
||||
path_angle_enabled: true
|
||||
path_angle_power: 1.0
|
||||
path_angle_weight: 2.0
|
||||
path_angle_max_angle_to_furthest: 1.2
|
||||
path_angle_threshold_to_consider: 0.5
|
||||
path_angle_offset_from_furthest: 4
|
||||
path_angle_mode: 0
|
||||
|
||||
path_follow_enabled: true
|
||||
path_follow_power: 1.0
|
||||
path_follow_weight: 5.0
|
||||
path_follow_threshold_to_consider: 1.4
|
||||
path_follow_offset_from_furthest: 6
|
||||
|
||||
prefer_forward_enabled: true
|
||||
prefer_forward_power: 1.0
|
||||
prefer_forward_weight: 1.0
|
||||
prefer_forward_threshold_to_consider: 0.5
|
||||
|
||||
twirling_enabled: false
|
||||
twirling_power: 1.0
|
||||
twirling_weight: 0.0
|
||||
twirling_threshold_to_consider: 0.5
|
||||
|
||||
velocity_deadband_enabled: false
|
||||
velocity_deadband_power: 1.0
|
||||
velocity_deadband_weight: 0.0
|
||||
velocity_deadband_deadband_velocities: [0.0, 0.0, 0.0]
|
||||
|
||||
publish_debug: false
|
||||
# --- Covariance thich nghi (giai doan 2) ---
|
||||
# Moi chu ky fit lai do rong nhieu cho TUNG buoc thoi gian tu chinh batch.
|
||||
# vx_std/wz_std o tren chi con la gia tri khoi tao va gia tri sau reset.
|
||||
adaptive_covariance: true
|
||||
gaussian_fitting_lambda: 5.0 # phai < param_lambda de trong so du nhon
|
||||
# San chon bang thuc nghiem: thap hon -> muot hon nhung cham va sat vat can hon.
|
||||
min_vx_std: 0.08
|
||||
max_vx_std: 0.35
|
||||
min_wz_std: 0.10
|
||||
max_wz_std: 0.60
|
||||
# Khi bi ket, no rong sigma dan qua tung chu ky de thoat cuc tieu dia phuong.
|
||||
stuck_sigma_inflation: 1.6
|
||||
hard_obstacle_cost_threshold: 254
|
||||
stage_xy_weight: 60.0
|
||||
stage_yaw_weight: 1.0
|
||||
stage_speed_weight: 12.0
|
||||
terminal_xy_weight: 90.0
|
||||
terminal_yaw_weight: 1.0
|
||||
terminal_speed_weight: 12.0
|
||||
@@ -1,8 +1,8 @@
|
||||
MQTT:
|
||||
Name: T800
|
||||
Host: 172.20.235.170
|
||||
Port: 1885
|
||||
Client_ID: T800
|
||||
Username: robotics
|
||||
Password: robotics
|
||||
Keep_Alive: 60
|
||||
# MQTT:
|
||||
# Name: T800
|
||||
# Host: 172.20.235.170
|
||||
# Port: 1885
|
||||
# Client_ID: T800
|
||||
# Username: robotics
|
||||
# Password: robotics
|
||||
# Keep_Alive: 60
|
||||
56
config/priest_local_planner_params.yaml
Normal file
56
config/priest_local_planner_params.yaml
Normal file
@@ -0,0 +1,56 @@
|
||||
PriestLocalPlanner:
|
||||
library_path: libpriest_local_planner
|
||||
|
||||
# ================= Horizon / optimizer =================
|
||||
t_fin: 7.0 # [s] planning horizon
|
||||
num_steps: 30 # trajectory samples over the horizon
|
||||
num_batch: 60 # trajectories per CEM iteration
|
||||
initial_up_sampling: 5 # candidate oversampling factor
|
||||
ellite_num: 40 # elites ranked by projection residual
|
||||
num_warm: 20 # warm-started trajectories between cycles
|
||||
max_proj_iter: 8 # alternating projection iterations
|
||||
max_cem_iter: 4 # CEM iterations per control cycle
|
||||
optimizer_threads: 4 # bounded workers; avoid severe overhead from using all CPU cores
|
||||
solve_time_warn_ms: 30.0 # [ms] leaves margin inside the 30 Hz controller period
|
||||
lateral_std: 0.8 # [m] std of lateral waypoint perturbations
|
||||
|
||||
# ================= Robot limits (differential drive) =================
|
||||
max_velocity: 0.5 # [m/s] forward velocity limit
|
||||
max_angular_velocity: 0.4 # [rad/s] also enforced inside the optimizer as a
|
||||
# lateral-acceleration clamp |a_n| <= omega_max * v
|
||||
max_acceleration: 0.5 # [m/s^2] used in the trajectory projection
|
||||
reference_velocity: 0.4 # [m/s] desired cruise speed along the path
|
||||
v_eps: 0.05 # [m/s] below this speed the curvature clamp/cost is skipped
|
||||
v_min_seed: 0.05 # [m/s] initial-velocity seed along yaw when the robot is stopped
|
||||
|
||||
# Trajectory -> differential (v, omega) conversion with curvature feedforward:
|
||||
# omega = clamp(omega_traj, +-max_angular_velocity) - heading_gain * heading_error
|
||||
heading_gain: 1.5 # [1/s] feedback gain on the heading error
|
||||
heading_error_stop: 1.2 # [rad] above this, rotate in place (v = 0)
|
||||
min_in_place_yawrate: 0.2 # [rad/s]
|
||||
|
||||
# ================= Goal / plan =================
|
||||
xy_goal_tolerance: 0.10 # [m]
|
||||
yaw_goal_tolerance: 0.10 # [rad]
|
||||
max_global_plan_lookahead_dist: 3.0 # [m]
|
||||
|
||||
# ================= Obstacles (from local costmap) =================
|
||||
num_obs: 40 # obstacles in the CEM cost
|
||||
num_obs_proj: 30 # closest obstacles inside the projection
|
||||
obstacle_range: 2.5 # [m] costmap scan radius around the robot
|
||||
robot_radius: 0.50 # [m]
|
||||
obstacle_point_radius: 0.08 # [m]
|
||||
footprint_padding: 0.05 # [m]
|
||||
obstacle_cost_threshold: 150
|
||||
obstacle_cell_step: 2
|
||||
max_obstacle_points: 150
|
||||
|
||||
# ================= Cost weights =================
|
||||
weight_smoothness: 0.1
|
||||
weight_track: 0.2
|
||||
weight_obs: 1.2
|
||||
weight_clearance: 3.0 # bounded proximity term in (0,1]: 1 at an obstacle
|
||||
# centre, 0.5 on the collision boundary, 0 out of range
|
||||
weight_angular: 0.5 # hinge penalty on |omega| > max_angular_velocity
|
||||
cem_lamda: 0.9 # CEM temperature
|
||||
cem_alpha: 0.7 # CEM mean/cov blending factor
|
||||
170
config/recovery_behaviors_params.yaml
Normal file
170
config/recovery_behaviors_params.yaml
Normal file
@@ -0,0 +1,170 @@
|
||||
# Bộ recovery behavior (gen-2, tick-based) và tham số vận hành của chúng.
|
||||
#
|
||||
# Đây là cây config RUNTIME. Bản dùng cho test của gói nằm ở
|
||||
# `Test/recovery_core/test/config/recovery_behaviors_params.yaml` — sửa tham số vận hành thì sửa ở
|
||||
# đây, đừng sửa bản test.
|
||||
#
|
||||
# Danh sách này được `recovery_core::RecoveryRegistry` đọc; caller (RecoveryRunner của move_base2)
|
||||
# truyền namespace `recovery` vào. Khoá `recovery_behaviors:` trong `move_base_common_params.yaml`
|
||||
# thuộc về move_base cũ và KHÔNG liên quan tới file này.
|
||||
|
||||
recovery:
|
||||
# `behaviors` là registry mọi plugin/instance CÓ THỂ dùng. Thứ tự ở đây chỉ là thứ tự nạp,
|
||||
# không còn quyết định luồng chạy. `routes` bên dưới chọn riêng chuỗi theo trigger.
|
||||
#
|
||||
# wait : không di chuyển. Vật cản động (người, xe khác) tự đi qua là xong.
|
||||
# clear : xoá vật cản đã tích trong costmap, gần trước rồi xa sau.
|
||||
# rotate : quay tại chỗ cho costmap nhìn lại xung quanh.
|
||||
# back_up: LÙI — hướng robot không có sensor, nên xếp cuối cùng.
|
||||
behaviors:
|
||||
- {name: wait, type: WaitRecovery}
|
||||
- {name: clear, type: ClearCostmapRecovery}
|
||||
# Đặt schema trước plugin: chưa có `DetourPathRecovery`/library_path thì RecoveryRunner bỏ
|
||||
# entry này cùng warning, và bỏ nó khỏi từng route. Khi plugin SBPL được thêm sau này, không
|
||||
# cần sửa move_base2 hay route.
|
||||
- {name: detour_path, type: DetourPathRecovery}
|
||||
- {name: rotate, type: RotateRecovery}
|
||||
- {name: back_up, type: BackUpRecovery}
|
||||
|
||||
# trigger -> tên behavior thử tuần tự; lần lỗi kế tiếp cùng trigger mới tiến tới phần tử tiếp
|
||||
# theo của route đó. Đường quay về sau khi một behavior kết thúc (contract 2026-08-04b):
|
||||
# - planning_failed -> quay về PLANNING lập plan lại (không có plan nào để bám);
|
||||
# - controlling/oscillation-> quay về CONTROLLING bám tiếp GLOBAL PATH CŨ, KHÔNG lập plan lại
|
||||
# (thay plan trong lúc chạy là việc riêng của planner_frequency);
|
||||
# riêng khi leo thang từ PLANNING vì mất pose thì về PLANNING;
|
||||
# - behavior họ detour (output kPath, vd DetourPathRecovery) nộp path mới -> path đó được áp
|
||||
# và robot bám tiếp — cách duy nhất một recovery được thay plan.
|
||||
# Robot đi được quá `oscillation_distance` thì mọi cursor route được reset — ngân sách recovery
|
||||
# đầy lại theo tiến độ thật (2026-08-04).
|
||||
#
|
||||
# `controlling_failed` từ 2026-08-04 nghĩa là MẤT DỮ LIỆU/LỆNH: controller không ra lệnh (kể cả
|
||||
# mất pose) quá controller_patience, hoặc mất pose ngay trong PLANNING. Route chỉ có [wait] là
|
||||
# chủ đích: đứng yên chờ một nhịp cho dữ liệu quay lại; hết wait mà dữ liệu vẫn chưa về thì
|
||||
# cursor cạn -> ABORTED, robot dừng hẳn và báo fail; dữ liệu về thì lập plan chạy tiếp.
|
||||
# `planning_failed` giữ đúng nghĩa "planner CÓ dữ liệu nhưng không tìm được đường".
|
||||
# `path_blocked` / `off_path` là TUỲ CHỌN: xoá hai dòng đó = tắt giám sát tuyến, runtime chạy y
|
||||
# như trước 2026-08-04d. Chúng dùng chung `detour_path` nhưng giữ cursor riêng — mỗi nguyên nhân
|
||||
# một ngân sách, và log phân biệt được vì sao robot dừng.
|
||||
routes:
|
||||
planning_failed: [wait, clear]
|
||||
controlling_failed: [detour_path]
|
||||
oscillation: [wait]
|
||||
# Cursor reset (2026-08-05): recovery THÀNH CÔNG + NỘP PATH MỚI (detour) thì cursor của trigger
|
||||
# đó reset NGAY — lần chắn kế tiếp là sự cố mới, detour được chạy lại. Từ chối/thất bại mới ăn
|
||||
# vào ngân sách route. Route được phép LẶP behavior nếu muốn cấp thêm lượt thử giữa các nhịp
|
||||
# wait/nhích-lại-gần.
|
||||
#
|
||||
# Cạn route KHÔNG còn nghĩa là ABORT (contract 2026-08-05e): `path_blocked`/`off_path` là DỰ
|
||||
# BÁO, plan vẫn hợp lệ và controller vẫn ra lệnh được, nên robot bám tiếp plan cũ và trigger bị
|
||||
# KHOÁ tới khi đi được quá `oscillation_distance`. Đường ABORT hợp lệ duy nhất đi qua
|
||||
# `controlling_failed` (controller thật sự hết ra lệnh quá controller_patience) hoặc
|
||||
# `planning_failed`.
|
||||
path_blocked: [detour_path]
|
||||
off_path: [detour_path]
|
||||
|
||||
wait:
|
||||
wait_duration: 3.0 # [s] đợi vật cản động đi qua
|
||||
|
||||
# Xoá vật cản trong vùng vuông cạnh reset_distance quanh robot.
|
||||
clear:
|
||||
reset_distance: 3.0 # [m] cạnh vùng xoá
|
||||
invert_area_to_clear: false # false = xoá BÊN TRONG vùng
|
||||
affected_maps: both # local | global | both
|
||||
layer_names: [obstacles] # phải khớp `plugins:` của costmap; sai tên -> log kèm tên layer thật
|
||||
|
||||
rotate:
|
||||
full_rotation: true # quay đủ 2*pi để costmap thấy toàn bộ xung quanh
|
||||
angular_speed: 0.4 # [rad/s] độ lớn; dấu do goal.angle quyết định
|
||||
acc_lim_theta: 0.8 # [rad/s^2] ramp, tránh giật khi có tải
|
||||
sim_granularity: 0.1 # [rad] bước quét footprint dọc cung lúc start
|
||||
timeout: 20.0 # [s] lưới cuối nếu robot bị giữ cơ học
|
||||
|
||||
back_up:
|
||||
backup_distance: 0.1 # [m] quãng lùi mặc định
|
||||
backup_distance_max: 1.0 # [m] trần cứng, chặn cả goal.distance lẫn param
|
||||
linear_speed: 0.1 # [m/s] ĐỘ LỚN; dấu âm (lùi) do plugin đặt
|
||||
acc_lim_x: 0.3 # [m/s^2]
|
||||
timeout: 15.0 # [s]
|
||||
|
||||
# Lập đường vòng quanh vật cản (gói sbpl_recovery, họ kPath) rồi NỐI LẠI plan hiện hành — đường
|
||||
# vòng được bám ngay, không replan (contract 2026-08-04b). Chưa nằm trong route nào ở trên: thêm
|
||||
# `detour_path` vào route cần nó (vd `controlling_failed: [wait, detour_path]`) để kích hoạt.
|
||||
detour_path:
|
||||
# Instance planner RIÊNG, không dùng chung SBPLLatticePlanner của backup: detour gọi makePlan
|
||||
# ĐỒNG BỘ trên control thread, nên `allocated_time` của instance này là trần treo control loop
|
||||
# mỗi tick. Xem khối SBPLDetourPlanner ở cuối file.
|
||||
planner_name: SBPLDetourPlanner # namespace param riêng (khối bên dưới)
|
||||
planner_symbol: SBPLLatticePlanner # alias Boost.DLL CÓ THẬT trong .so — libsbpl_lattice_planner
|
||||
# chỉ export đúng một symbol này. Khác bộ local planner, nơi
|
||||
# mỗi biến thể là một class riêng nên tên instance trùng alias.
|
||||
# "combined" (2026-08-05, vòng 3 — LƯỚI GỘP, xem DETOUR_MERGED_GRID_PLAN.md): snapshot global
|
||||
# làm nền (tường/kệ tĩnh toàn bản đồ) + đè cửa sổ local lên (vật cản sensor tươi), SBPL lập
|
||||
# đường TRÊN BẢN CHỤP GỘP — không giữ mutex costmap nào trong lúc search, map update không bị
|
||||
# bỏ đói dù attempt thua. Một bên báo chắn là chắn; local FREE không xoá tường global.
|
||||
# Cần planner export capability "<planner_symbol>ExternalGrid" (sbpl_lattice_planner có sẵn).
|
||||
# "local" / "global" đơn lẻ vẫn dùng được cho hệ thiếu một trong hai nguồn.
|
||||
costmap_source: combined
|
||||
planning_distance: 1.0 # [m] điểm nối cách robot tối thiểu chừng này dọc plan.
|
||||
# [m] TRẦN quét dọc plan, thuần tuỳ chọn. 0 = quét tới CUỐI PLAN (lưới gộp phủ toàn bản đồ nên
|
||||
# không còn ràng buộc cửa sổ local). Quét luôn tự dừng ở rìa lưới; hai lần sim 2026-08-05 cap
|
||||
# hữu hạn (3.8 rồi 8.0) đều cắt quét ngay sau pose bẩn cuối và báo "không có chỗ sạch" oan.
|
||||
max_rejoin_distance: 0.0
|
||||
sample_step: 0.5 # [m] khoảng cách dọc plan giữa hai ứng viên điểm nối
|
||||
# [m] Điểm nối phải cách pose bẩn gần nhất CẢ HAI PHÍA dọc plan chừng này, và nằm SAU CỤM CHẮN
|
||||
# ĐẦU TIÊN (không phải pose bẩn cuối toàn tầm — vết bẩn xa trên đuôi plan cũ không được giết
|
||||
# ứng viên hợp lệ giữa hai vật cản; sim 11:00: `last blocked at 7.96 m` -> refuse oan -> ABORT).
|
||||
rejoin_clearance: 0.5
|
||||
attempts_per_run: 3 # số ứng viên thử tối đa; mỗi control cycle thử MỘT ứng viên
|
||||
# Tuyến để NỐI LẠI (2026-08-05e). "reference" = tuyến GỐC của chặng, tức plan do planner sinh và
|
||||
# recovery không thay được: đường vòng luôn quay về tuyến order, và `max_deviation` đo đúng hành
|
||||
# lang fleet đã duyệt. "current" = plan đang bám (hành vi cũ) — sim đo được tuyến bị ăn mòn dần
|
||||
# qua từng lượt detour (rejoin 106/305 -> 130/289 -> 132/273) và robot không bao giờ về tuyến.
|
||||
rejoin_on: reference
|
||||
# [m] Hành lang lệch tuyến cho phép quanh tuyến gốc của order. 0 = KHÔNG giới hạn — giữ 0 cho
|
||||
# tới khi biết dung sai thật của fleet master; đặt số mò còn tệ hơn tắt.
|
||||
max_deviation: 0.0
|
||||
timeout: 10.0 # [s] trần lượt
|
||||
|
||||
# Bảng symbol -> thư viện cho Boost.DLL. Thiếu khoá library_path là nguyên nhân phổ biến nhất của
|
||||
# lỗi "plugin build xong nhưng runtime báo không tìm thấy".
|
||||
WaitRecovery:
|
||||
library_path: librecovery_core_wait_recovery
|
||||
|
||||
ClearCostmapRecovery:
|
||||
library_path: librecovery_core_clear_costmap_recovery
|
||||
|
||||
RotateRecovery:
|
||||
library_path: librecovery_core_rotate_recovery
|
||||
|
||||
BackUpRecovery:
|
||||
library_path: librecovery_core_back_up_recovery
|
||||
|
||||
DetourPathRecovery:
|
||||
library_path: libsbpl_recovery
|
||||
|
||||
# Instance SBPL RIÊNG cho detour — cùng .so với SBPLLatticePlanner, khác tuning.
|
||||
#
|
||||
# Vì sao không dùng chung: `DetourPathRecovery::onUpdate` gọi makePlan ĐỒNG BỘ trên control thread,
|
||||
# nên `allocated_time` ở đây là trần thời gian control loop bị treo mỗi tick — `cancel`/`pause` từ
|
||||
# host không được xử lý trong khoảng đó. Bản backup để 10.0 s là hợp lý cho vai trò của nó, nhưng
|
||||
# treo loop 10 s thì không.
|
||||
#
|
||||
# `initial_epsilon` lớn = ARA* trả nghiệm đầu rất nhanh, đường dài hơn tối ưu nhưng đây là đường
|
||||
# vòng tạm — nhanh quan trọng hơn ngắn.
|
||||
SBPLDetourPlanner:
|
||||
# KHÔNG có `library_path` ở đây: `SBPLDetourPlanner` không phải symbol, nó chỉ là namespace param.
|
||||
# Việc nạp .so đi theo `planner_symbol: SBPLLatticePlanner` ở trên.
|
||||
environment_type: XYThetaLattice
|
||||
planner_type: ARAPlanner
|
||||
allocated_time: 0.4 # [s] TRẦN CỨNG cho một tick recovery
|
||||
initial_epsilon: 2.0
|
||||
force_scratch_limit: 10000
|
||||
forward_search: true
|
||||
# false cho DETOUR, khác bản backup: free heading làm mỗi lượt hỏng chạy HAI lần search (log sim:
|
||||
# "no solution with free start heading, retrying..." = 0.77 s giữ mutex costmap, map update miss
|
||||
# nhịp). Detour chạy khi robot ĐANG BÁM plan nên heading thật đã xuôi theo tuyến — không cần nới.
|
||||
free_start_heading: true
|
||||
nominalvel_mpersecs: 0.3
|
||||
timetoturn45degsinplace_secs: 1.31
|
||||
# Cùng file .mprim với SBPLLatticePlanner: resolution 0.05 m khớp resolution costmap local.
|
||||
primitive_filename: /home/duongtd/rl_ws/mir_robot/mir_navigation/mprim/unicycle_highcost_5cm.mprim
|
||||
@@ -6,6 +6,10 @@ SBPLLatticePlanner:
|
||||
initial_epsilon: 1.0
|
||||
force_scratch_limit: 10000
|
||||
forward_search: true
|
||||
# Bỏ ràng buộc heading xuất phát: local planner đã có bước quay tại chỗ đầu path
|
||||
# (turn_around_priority) nên không cần SBPL vẽ cung quay đầu khi goal ở phía sau.
|
||||
# Nếu không ra nghiệm, planner tự retry một lần với heading thật của robot.
|
||||
free_start_heading: true
|
||||
nominalvel_mpersecs: 0.3
|
||||
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
|
||||
primitive_filename: /home/duongtd/T800_ws/src/AMR_T800/Test/sbpl_lattice_planner/matlab/mprim/unicycle_highcost_5cm.mprim
|
||||
primitive_filename: /home/duongtd/rl_ws/mir_robot/mir_navigation/mprim/unicycle_highcost_5cm.mprim
|
||||
@@ -1,14 +1,115 @@
|
||||
LocalPlannerAdapter:
|
||||
library_path: liblocal_planner_adapter
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.03
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.03
|
||||
min_approach_linear_velocity: 0.06
|
||||
|
||||
StanleyLocalPlanner:
|
||||
# Algorithm
|
||||
library_path: libstanley_local_planner
|
||||
|
||||
StanleyDockingLocalPlanner:
|
||||
# Algorithm
|
||||
# ================= Robot limits =================
|
||||
max_vel_x: 0.40 # [m/s] forward, > 0
|
||||
min_vel_x: -0.25 # [m/s] reverse, < 0
|
||||
wheel_base: 0.0 # [m] 0 = derive from TF front_axle_frame -> rear_axle_frame
|
||||
front_axle_frame: "steer_link"
|
||||
rear_axle_frame: "base_link"
|
||||
|
||||
# Two SEPARATE steering limits.
|
||||
# Path tracking must not use a large angle: forward_vel_ scales with cos(delta) so it
|
||||
# collapses to speed_base, and vel_steer = v/cos(steer) is guarded to 0 as cos -> 0.
|
||||
# Pivoting in place uses the full 90 deg the hardware has.
|
||||
steer_limit_standstill: 1.5708 # [rad] TO BE MEASURED - ceiling at |v| ~ 0
|
||||
steer_limit_at_max_vel: 0.5000 # [rad] TO BE MEASURED - ceiling at |v| = max_vel_x
|
||||
align_steer_angle: 1.5708 # [rad] steering while pivoting. Literal number, NOT M_PI/2
|
||||
min_steer_angle: 0.20 # [rad] threshold for "steering is engaged"
|
||||
max_steer_rate: 0.90 # [rad/s] TO BE MEASURED - steering servo slew
|
||||
|
||||
# ================= Goal tolerance =================
|
||||
xy_goal_tolerance: 0.030 # [m] should match LocalPlannerAdapter
|
||||
yaw_goal_tolerance: 0.020 # [rad]
|
||||
goal_sq_dist_tol: 0.05 # [m^2] early exit of the closest-point scan
|
||||
|
||||
# ================= Control law (Stanley) =================
|
||||
path_yaw_mode: "tangent" # tangent | bearing - "bearing" is the old behaviour, kept for rollback
|
||||
control_gain_e: 2.0 # heading gain (path tangent - robot yaw)
|
||||
control_gain_d: 1.0 # base cross-track gain
|
||||
gain_cte_weight: 0.8 # added to k per |cross-track error| [1/m]
|
||||
gain_heading_weight: 0.5 # added to k per |heading error| [1/rad]
|
||||
gain_min: 0.5 # lower clamp of k
|
||||
gain_max: 1.8 # upper clamp of k
|
||||
soft_min_vel: 0.20 # [m/s] FLOOR of the atan2(k*e, v) denominator. NO ceiling
|
||||
alpha_gain: 0.0 # steering low-pass [0..1]; 0 = off, max_steer_rate is used instead
|
||||
curve_gain_e_bonus: 2.0 # added to control_gain_e in curve mode
|
||||
tangent_window_pts: 3 # points averaged for the tangent when the plan carries no yaw
|
||||
|
||||
# ================= Steering bias (kills the steady-state offset) =================
|
||||
# Enable when the robot tracks parallel to the global path because of a mechanical
|
||||
# zero offset or an IMU bias. Start at 0 and raise by 0.005 per trial.
|
||||
steer_bias_gain: 0.0 # [rad/(m*s)] 0 = OFF. Suggested when enabled: 0.02
|
||||
steer_bias_clamp: 0.05 # [rad] anti-windup ceiling
|
||||
steer_bias_decay: 0.995 # per-cycle bleed when the integrate conditions do not hold
|
||||
steer_bias_min_vel: 0.30 # [m/s] below this, do not integrate
|
||||
steer_bias_max_heading_err: 0.10 # [rad] above this, do not integrate
|
||||
|
||||
# ================= Plan window / lookahead =================
|
||||
max_global_plan_lookahead_dist: 3.0 # [m] window ceiling
|
||||
min_lookahead: 0.50 # [m] window floor
|
||||
min_lookahead_near_goal: 0.40 # [m] separate floor near the goal
|
||||
dyn_lookahead_gain: 2.50 # [m/(m/s)] lookahead = min + gain*|v|
|
||||
near_goal_freeze_dist: 0.50 # [m] below this: freeze yaw_path, run theta_d only
|
||||
|
||||
# Index continuity thresholds, given in METRES and converted to indices with the
|
||||
# point spacing measured at runtime - the plan resolution is the global planner's
|
||||
# choice, not a constant.
|
||||
# The old code used fixed indices (back=3, fwd=15 ~ 0.75 m on a 0.05 m/point plan) and
|
||||
# therefore never fired: at 1.2 m/s the robot covers 0.04 m = 0.8 index per 33 ms cycle.
|
||||
# Applied to BOTH closest (window anchor) and target_idx_ (inside the core).
|
||||
max_advance_dist: 0.20 # [m] forward travel allowed along the plan per cycle
|
||||
max_back_dist: 0.10 # [m] backward travel allowed along the plan per cycle
|
||||
|
||||
# ================= Curvature =================
|
||||
high_curvature: 0.30 # [1/m] above: shrink lookahead
|
||||
mid_curvature: 0.20 # [1/m] above: cut the transform window
|
||||
low_curvature: 0.10 # [1/m] above: enter curve mode
|
||||
straight_confirm_steps: 110 # consecutive straight cycles before leaving curve mode
|
||||
|
||||
# ================= Speed profile =================
|
||||
speed_base: 0.30 # [m/s] base of the cosine steering profile
|
||||
curve_max_speed: 0.25 # [m/s] ceiling while curving
|
||||
min_drive_speed: 0.07 # [m/s] floor while driving
|
||||
accel_distance: 2.00 # [m] ramp length after leaving a curve
|
||||
decel_min: 0.05 # [m/s per cycle] deceleration on predicted collision
|
||||
decel_max: 0.10 # [m/s per cycle]
|
||||
collision_horizon_factor: 2.0 # multiplies max_vel_x to get the collision check distance
|
||||
|
||||
# ================= Align / phase =================
|
||||
heading_eps: 0.020 # [rad] heading error accepted as aligned
|
||||
steer_eps: 0.020 # [rad] steering error accepted as "servo arrived"
|
||||
heading_align_tol: 0.050 # [rad] gate for the final straight-line approach (was 0.01 = too tight)
|
||||
near_goal_dist: 1.00 # [m] below this: enable the final straight mode
|
||||
steer_realign_off: 0.503 # [rad] above this: CUT drive (0.16*pi)
|
||||
steer_realign_on: 0.150 # [rad] below this: RESUME drive - must be smaller than _off
|
||||
steering_active: 0.094 # [rad] above this the steering counts as active (0.03*pi)
|
||||
segment_transition_threshold: 0.010 # [m]
|
||||
segment_end_ratio: 0.05 # fraction of the segment left before advancing
|
||||
xy_reached_gate_factor: 3.0 # multiplies xy_goal_tolerance; a 1-point window may only latch inside this
|
||||
|
||||
# ================= TF guard =================
|
||||
# Measured on the robot: 225 map->odom jumps, max 12.381 m, coinciding with the
|
||||
# localisation FinishTrajectory(1) 10:41:18 / AddTrajectory(2) 10:41:21. The source is
|
||||
# outside this package; this is containment, not a cure.
|
||||
tf_lookup_at_pose_stamp: true # true = look up at pose.header.stamp instead of "latest".
|
||||
# "latest" returns a different entry each call when the
|
||||
# buffer holds two map->odom transforms
|
||||
transform_tolerance: 0.20 # [s] accepted age when looking up by timestamp
|
||||
max_plan_tf_jump_rate: 0.60 # [m/s] threshold as a RATE, multiplied by the real elapsed time
|
||||
max_plan_tf_yaw_jump: 0.20 # [rad] rotation threshold - the old code checked translation only
|
||||
tf_reject_window_cycles: 30 # sliding window; exceeding it means a real relocalisation
|
||||
tf_reject_ratio: 0.5 # reject fraction inside the window that triggers the reset
|
||||
|
||||
# ================= Debug =================
|
||||
debug_level: 1 # 0=off 1=on state change 2=throttled 1 Hz 3=every cycle
|
||||
# 3 produces >2000 lines/min at 30 Hz - field debugging only
|
||||
|
||||
StanleyDockingLocalPlanner:
|
||||
library_path: libstanley_local_planner
|
||||
|
||||
Reference in New Issue
Block a user