HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add recovery and mission manager structure

Browse Source
This commit is contained in:
duongtd
2026-07-03 17:37:39 +07:00
parent 33d6537947
commit 565e18bfb8
21 changed files with 349 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
config/costmap_common_params.yaml
View File

@@ -11,7 +11,7 @@ navigation_map:
map_file: maze
virtual_walls_map:
map_topic: /virtual_walls/map
map_topic: /map
namespace: /virtual_walls
map_pkg: managerments
map_file: maze
@@ -19,23 +19,23 @@ virtual_walls_map:
lethal_cost_threshold: 100
obstacles:
observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
f_scan_marking:
topic: /f_scan
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: true
min_obstacle_height: 0.0
max_obstacle_height: 0.25
f_scan_clearing:
topic: /f_scan
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: true
min_obstacle_height: 0.0
max_obstacle_height: 0.25
observation_sources: b_scan_marking b_scan_clearing pc_marking pc_clearing
# f_scan_marking: f_scan_marking f_scan_clearing
# topic: /f_scan
# data_type: LaserScan
# clearing: false
# marking: true
# inf_is_valid: true
# min_obstacle_height: 0.0
# max_obstacle_height: 0.25
# f_scan_clearing:
# topic: /f_scan
# data_type: LaserScan
# clearing: true
# marking: false
# inf_is_valid: true
# min_obstacle_height: 0.0
# max_obstacle_height: 0.25
b_scan_marking:
topic: /b_scan
data_type: LaserScan
@@ -53,4 +53,28 @@ obstacles:
min_obstacle_height: 0.0
max_obstacle_height: 0.25
pc_marking:
topic: /camera/depth/points_proc
data_type: PointCloud2
clearing: false
marking: true
inf_is_valid: false
observation_persistence: 0.0
expected_update_rate: 0.5
obstacle_range: 2.5
raytrace_range: 3.0
min_obstacle_height: 0.1
max_obstacle_height: 1.0
pc_clearing:
topic: /camera/depth/points_clear
data_type: PointCloud2
clearing: true
marking: false
inf_is_valid: 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

2
config/costmap_global_params.yaml
View File

@@ -4,7 +4,7 @@ global_costmap:
global_frame: map
update_frequency: 1.0
publish_frequency: 1.0
raytrace_range: 2.0
raytrace_range: 3.0
resolution: 0.05
z_resolution: 0.2
rolling_window: false

2
config/costmap_global_params_plugins_no_virtual_walls.yaml
View File

@@ -7,4 +7,4 @@ global_costmap:
- {name: inflation, type: "InflationLayer" }
obstacles:
enabled: false
footprint_clearing_enabled: false
footprint_clearing_enabled: true

2
config/costmap_local_params.yaml
View File

@@ -5,7 +5,7 @@ local_costmap:
update_frequency: 6.0
publish_frequency: 6.0
rolling_window: true
raytrace_range: 2.0
raytrace_range: 3.0
resolution: 0.05
z_resolution: 0.15
z_voxels: 8

1
config/costmap_local_params_plugins_no_virtual_walls.yaml
View File

@@ -1,6 +1,7 @@
local_costmap:
frame_id: odom
plugins:
# - {name: virtual_walls_map, type: "StaticLayer" }
- {name: obstacles, type: "VoxelLayer" }
- {name: inflation, type: "InflationLayer" }
obstacles:

64
config/hybrid_local_planner_params.yaml Normal file
View File

@@ -0,0 +1,64 @@
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
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
# GoalTolerance
xy_goal_tolerance: 0.02
yaw_goal_tolerance: 0.02
# Trajectory
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
to_goal_cost_gain: 0.8
speed_cost_gain: 0.8
# Obstacles
obs_range: 2.5
#GeneralSetting
segment_transition_threshold: 0.01
calibration_factor: 1.5
use_obstacle_avoidance: true
slow_velocity_th: 0.1
turn_direction_th: 0.1

35
config/move_base_common_params.yaml
View File

@@ -1,18 +1,43 @@
position_planner_name: PNKXLocalPlanner
# 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
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: LocalPlannerAdapter
base_global_planner: CustomPlanner
PNKXLocalPlanner:
# 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:
base_local_planner: LocalPlannerAdapter
base_global_planner: CustomPlanner
base_global_planner: SBPLLatticePlanner
PNKXDockingLocalPlanner:
base_local_planner: LocalPlannerAdapter
base_global_planner: TwoPointsPlanner
PNKXGoStraightLocalPlanner:
base_local_planner: LocalPlannerAdapter
base_global_planner: TwoPointsPlanner
@@ -30,10 +55,10 @@ 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: false
recovery_behavior_enabled: true
recovery_behaviors: [
{name: aggressive_reset, type: ClearCostmapRecovery},
{name: conservative_reset, type: ClearCostmapRecovery},
{name: conservative_reset, type: ClearCostmapRecovery}
]
conservative_reset:

126
config/mppi_local_planner_params.yaml Normal file
View File

@@ -0,0 +1,126 @@
LocalPlannerAdapter:
library_path: liblocal_planner_adapter
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.03
min_approach_linear_velocity: 0.06
MPPILocalPlanner:
library_path: libmppi_local_planner
# Robot limits [m/s, rad/s, m/s^2, rad/s^2]
max_velocity: 0.5
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
vx_std: 0.2
vy_std: 0.2
wz_std: 0.4
motion_model: 0
min_turning_radius: 0.2
# 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
max_global_plan_lookahead_dist: 3.0
global_plan_prune_distance: 0.2
sim_period: 0.1
# Costmap safety
use_costmap_collision_check: true
allow_unknown: true
lethal_cost: 253
inflation_score_distance: 0.3
consider_footprint: true
# Critic defaults ported from the legacy MPPI dynamic_reconfigure files.
constraint_enabled: true
constraint_power: 1.0
constraint_weight: 4.0
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

11
config/sbpl_global_params.yaml Normal file
View File

@@ -0,0 +1,11 @@
SBPLLatticePlanner:
library_path: libsbpl_lattice_planner
environment_type: XYThetaLattice
planner_type: ARAPlanner
allocated_time: 10.0
initial_epsilon: 1.0
force_scratch_limit: 10000
forward_search: 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

14
config/stanley_local_planner_params.yaml Normal file
View File

@@ -0,0 +1,14 @@
LocalPlannerAdapter:
library_path: liblocal_planner_adapter
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
library_path: libstanley_local_planner