zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

给轨迹+MPPI版导航加入了障碍物检测

Browse Source
This commit is contained in:
Orange
2026-08-05 22:15:35 +08:00
parent b4689b940f
commit 66acea5e87
9 changed files with 1900 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/navigation/obstacle_nav2/config/nav2_params.yaml
View File

@@ -112,8 +112,8 @@ controller_server:
CostCritic:
enabled: true
cost_power: 1
cost_weight: 4.5
critical_cost: 300.0
cost_weight: 6.0
critical_cost: 250.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 0.8
@@ -134,7 +134,7 @@ controller_server:
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 16.0
cost_weight: 8.0
max_path_occupancy_ratio: 0.07
trajectory_point_step: 4
threshold_to_consider: 0.45
@@ -144,14 +144,14 @@ controller_server:
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 6.0
cost_weight: 4.0
offset_from_furthest: 4
threshold_to_consider: 1.0
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 6.0
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 0.45
max_angle_to_furthest: 1.2
@@ -160,7 +160,7 @@ controller_server:
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
cost_weight: 1.0
threshold_to_consider: 0.4
controller_server_rclcpp_node:
@@ -188,12 +188,13 @@ local_costmap:
plugin: "obstacle_nav2::ObstacleArrayLayer"
enabled: true
topic: /obstacles
obstacle_timeout: 0.5
obstacle_timeout: 1.0
transform_tolerance: 0.2
default_obstacle_radius: 0.05
minimum_obstacle_radius: 0.02
maximum_obstacle_radius: 0.06
extra_inflation: 0.02
retain_previous_on_empty_snapshot: true
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
@@ -209,8 +210,8 @@ local_costmap:
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 2.0
publish_frequency: 1.0
update_frequency: 5.0
publish_frequency: 3.0
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_footprint
@@ -227,12 +228,13 @@ global_costmap:
plugin: "obstacle_nav2::ObstacleArrayLayer"
enabled: true
topic: /obstacles
obstacle_timeout: 0.5
obstacle_timeout: 1.0
transform_tolerance: 0.2
default_obstacle_radius: 0.05
minimum_obstacle_radius: 0.02
maximum_obstacle_radius: 0.50
extra_inflation: 0.02
retain_previous_on_empty_snapshot: true
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
@@ -264,10 +266,10 @@ planner_server:
analytic_expansion_max_length: 2.0
minimum_turning_radius: 0.60
reverse_penalty: 2.2
change_penalty: 0.5
non_straight_penalty: 0.85
cost_penalty: 4.5
retrospective_penalty: 0.015
change_penalty: 0.0
non_straight_penalty: 0.8
cost_penalty: 5.0
retrospective_penalty: 0.01
lookup_table_size: 5.0
cache_obstacle_heuristic: false
viz_expansions: false

334
src/navigation/obstacle_nav2/config/nav2_profile_10.yaml Normal file
View File

@@ -0,0 +1,334 @@
# ============================================================================
# nav2_params.yaml — Odometry-only obstacle navigation
#
# No static map, no AMCL, no SLAM.
# Both costmaps are rolling windows in odom. The launch file can rewrite every
# global_frame leaf when a different connected odometry frame is required.
# Global planner: Smac Hybrid A* (Reeds-Shepp)
# Local controller: MPPI (Ackermann)
# ============================================================================
bt_navigator:
ros__parameters:
use_sim_time: False
global_frame: odom
robot_base_frame: base_footprint
odom_topic: /odom_combined
bt_loop_duration: 50
default_server_timeout: 20
# Injected by obstacle_nav2.launch.py from this package's share directory.
default_nav_to_pose_bt_xml: ""
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: False
controller_server:
ros__parameters:
use_sim_time: False
controller_frequency: 20.0
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 36
model_dt: 0.05
batch_size: 1000
vx_std: 0.75
vy_std: 0.0
wz_std: 0.4
vx_max: 1.75
vx_min: -0.75
vy_max: 0.0
wz_max: 1.5
iteration_count: 1
temperature: 0.3
gamma: 0.015
motion_model: "Ackermann"
visualize: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.4
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: false
cost_power: 1
cost_weight: 0.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
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
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: false
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: False
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_footprint
use_sim_time: False
rolling_window: true
width: 3
height: 3
resolution: 0.05
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
footprint_padding: 0.02
track_unknown_space: false
plugins: ["obstacle_array_layer", "inflation_layer"]
obstacle_array_layer:
plugin: "obstacle_nav2::ObstacleArrayLayer"
enabled: true
topic: /obstacles
obstacle_timeout: 0.5
transform_tolerance: 0.2
default_obstacle_radius: 0.05
minimum_obstacle_radius: 0.02
maximum_obstacle_radius: 0.50
extra_inflation: 0.02
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_footprint
use_sim_time: False
rolling_window: true
width: 10
height: 10
resolution: 0.05
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
footprint_padding: 0.02
track_unknown_space: false
plugins: ["obstacle_array_layer", "inflation_layer"]
obstacle_array_layer:
plugin: "obstacle_nav2::ObstacleArrayLayer"
enabled: true
topic: /obstacles
obstacle_timeout: 0.5
transform_tolerance: 0.2
default_obstacle_radius: 0.05
minimum_obstacle_radius: 0.02
maximum_obstacle_radius: 0.50
extra_inflation: 0.02
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
use_sim_time: False
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
downsample_costmap: false
downsampling_factor: 1
tolerance: 0.25
allow_unknown: false
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: "REEDS_SHEPP"
angle_quantization_bins: 72
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 3.0
minimum_turning_radius: 0.40
reverse_penalty: 1.0
change_penalty: 0.0
non_straight_penalty: 1.2
cost_penalty: 2.0
retrospective_penalty: 0.015
# 5 m covers the rolling planning horizon without the startup and memory
# cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
lookup_table_size: 5.0
cache_obstacle_heuristic: false
viz_expansions: false
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
do_refinement: true
refinement_num: 2
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: False
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:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
backup_dist: 0.8
backup_speed: 0.18
wait:
plugin: "nav2_behaviors/Wait"
wait_duration: 0.5
global_frame: odom
robot_base_frame: base_footprint
transform_tolerance: 0.5
use_sim_time: False
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: False
waypoint_follower:
ros__parameters:
loop_rate: 20
use_sim_time: False
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
smoothing_frequency: 20.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [1.75, 0.0, 1.5]
min_velocity: [-0.75, 0.0, -1.5]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: /odom_combined
odom_duration: 0.1
deadband_velocity: [0.03, 0.0, 0.03]
velocity_timeout: 1.0

358
src/navigation/obstacle_nav2/config/nav2_profile_11.yaml Normal file
View File

@@ -0,0 +1,358 @@
# ============================================================================
# nav2_params_basic_tracking.yaml
#
# Basic stable trajectory-following profile for obstacle_nav2.
# This file is intentionally conservative: low speed, longer MPPI horizon,
# smoother acceleration, moderate path tracking weights, and Ackermann limits.
#
# It does not replace nav2_params.yaml. To use it, launch Nav2 with this params
# file or add a launch argument later.
# ============================================================================
bt_navigator:
ros__parameters:
use_sim_time: False
global_frame: odom
robot_base_frame: base_footprint
odom_topic: /odom_combined
bt_loop_duration: 50
default_server_timeout: 20
default_nav_to_pose_bt_xml: ""
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: False
controller_server:
ros__parameters:
use_sim_time: False
controller_frequency: 20.0
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
# Longer horizon and moderate sampling make turns easier to discover.
time_steps: 48
model_dt: 0.05
batch_size: 1000
iteration_count: 1
# Conservative velocity exploration. Speed is deliberately not optimized.
vx_std: 0.20
vy_std: 0.0
wz_std: 0.85
vx_max: 0.50
vx_min: -0.15
vy_max: 0.0
wz_max: 1.8
# Lower temperature makes selection steadier; gamma damps rough controls.
temperature: 0.25
gamma: 0.02
motion_model: "Ackermann"
visualize: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.60
critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
CostCritic:
enabled: true
cost_power: 1
cost_weight: 6.0
critical_cost: 250.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 0.8
trajectory_point_step: 2
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
threshold_to_consider: 1.0
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
threshold_to_consider: 0.45
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 8.0
max_path_occupancy_ratio: 0.07
trajectory_point_step: 4
threshold_to_consider: 0.45
offset_from_furthest: 8
use_path_orientations: false
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
offset_from_furthest: 4
threshold_to_consider: 1.0
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 0.45
max_angle_to_furthest: 1.2
forward_preference: false
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 1.0
threshold_to_consider: 0.4
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: False
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 8.0
publish_frequency: 4.0
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_footprint
use_sim_time: False
rolling_window: true
width: 4
height: 4
resolution: 0.05
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
footprint_padding: 0.02
track_unknown_space: false
plugins: ["obstacle_array_layer", "inflation_layer"]
obstacle_array_layer:
plugin: "obstacle_nav2::ObstacleArrayLayer"
enabled: true
topic: /obstacles
obstacle_timeout: 1.0
transform_tolerance: 0.2
default_obstacle_radius: 0.05
minimum_obstacle_radius: 0.02
maximum_obstacle_radius: 0.06
extra_inflation: 0.02
retain_previous_on_empty_snapshot: true
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.25
always_send_full_costmap: True
local_costmap_client:
ros__parameters:
use_sim_time: False
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 3.0
transform_tolerance: 0.5
global_frame: odom
robot_base_frame: base_footprint
use_sim_time: False
rolling_window: true
width: 10
height: 10
resolution: 0.05
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
footprint_padding: 0.02
track_unknown_space: false
plugins: ["obstacle_array_layer", "inflation_layer"]
obstacle_array_layer:
plugin: "obstacle_nav2::ObstacleArrayLayer"
enabled: true
topic: /obstacles
obstacle_timeout: 1.0
transform_tolerance: 0.2
default_obstacle_radius: 0.05
minimum_obstacle_radius: 0.02
maximum_obstacle_radius: 0.50
extra_inflation: 0.02
retain_previous_on_empty_snapshot: true
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.45
always_send_full_costmap: True
global_costmap_client:
ros__parameters:
use_sim_time: False
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: False
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
use_sim_time: False
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
downsample_costmap: false
downsampling_factor: 1
tolerance: 0.20
allow_unknown: false
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 5.0
motion_model_for_search: "REEDS_SHEPP"
angle_quantization_bins: 72
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 2.0
minimum_turning_radius: 0.60
reverse_penalty: 2.2
change_penalty: 0.0
non_straight_penalty: 0.8
cost_penalty: 5.0
retrospective_penalty: 0.01
lookup_table_size: 5.0
cache_obstacle_heuristic: false
viz_expansions: false
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.35
w_data: 0.25
tolerance: 1.0e-10
do_refinement: true
refinement_num: 2
planner_server_rclcpp_node:
ros__parameters:
use_sim_time: False
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:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
backup_dist: 0.30
backup_speed: 0.08
wait:
plugin: "nav2_behaviors/Wait"
wait_duration: 0.5
global_frame: odom
robot_base_frame: base_footprint
transform_tolerance: 0.5
use_sim_time: False
simulate_ahead_time: 2.0
max_rotational_vel: 0.8
min_rotational_vel: 0.2
rotational_acc_lim: 1.5
robot_state_publisher:
ros__parameters:
use_sim_time: False
waypoint_follower:
ros__parameters:
loop_rate: 20
use_sim_time: False
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
smoothing_frequency: 20.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.50, 0.0, 1.8]
min_velocity: [-0.15, 0.0, -1.8]
max_accel: [0.50, 0.0, 1.50]
max_decel: [-0.60, 0.0, -1.80]
odom_topic: /odom_combined
odom_duration: 0.1
deadband_velocity: [0.02, 0.0, 0.02]
velocity_timeout: 1.0
lifecycle_manager_navigation:
ros__parameters:
use_sim_time: False
autostart: True
service_timeout: 10.0
bond_timeout: 8.0

27
src/navigation/obstacle_nav2/config/trajectory_guard.yaml Normal file
View File

@@ -0,0 +1,27 @@
trajectory_guard_node:
ros__parameters:
input_path_topic: /trajectory_guard/input_path
patched_path_topic: /trajectory_guard/patched_path
costmap_topic: /local_costmap/costmap
odom_topic: /odom_combined
planner_action: /compute_path_to_pose
follow_action: /follow_path
planner_id: GridBased
controller_id: FollowPath
goal_checker_id: ""
check_period_sec: 0.5
lookahead_distance: 2.0
rejoin_min_distance: 1.5
rejoin_max_distance: 5.0
occupied_threshold: 50
treat_unknown_as_occupied: true
footprint_half_length: 0.14
footprint_half_width: 0.085
footprint_padding: 0.04
footprint_sample_step: 0.05
repair_cooldown_sec: 0.5
blocked_retry_wait_sec: 0.8
wait_for_planner_costmap_update: true
min_repair_progress_indices: 5
publish_zero_on_blocked: true
cmd_vel_topic: /cmd_vel

7
src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py
View File

@@ -24,6 +24,7 @@ from launch.actions import (
DeclareLaunchArgument,
GroupAction,
IncludeLaunchDescription,
SetEnvironmentVariable,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -49,6 +50,7 @@ def generate_launch_description():
start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_params.yaml')
fastdds_profile_path = os.path.join(pkg_dir, 'config', 'fastdds_udp_only.xml')
nav_to_pose_bt_path = os.path.join(
pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
configured_params = RewrittenYaml(
@@ -103,6 +105,7 @@ def generate_launch_description():
'autostart': 'true',
}.items(),
)
# ========================== Assembly ======================================
return LaunchDescription([
DeclareLaunchArgument(
@@ -138,6 +141,10 @@ def generate_launch_description():
default_value='true',
description='Also start the obstacle_scanner node'),
SetEnvironmentVariable(
name='FASTRTPS_DEFAULT_PROFILES_FILE',
value=fastdds_profile_path),
safe_base_bringup,
lslidar_launch,
obstacle_scanner_launch,

265
src/navigation/obstacle_nav2/launch/obstacle_nav2_q_waypoints.launch.py Normal file
View File

@@ -0,0 +1,265 @@
#!/usr/bin/env python3
"""
obstacle_nav2_q_waypoints.launch.py
====================================
复合启动文件：同时启动 obstacle_nav2 导航系统 + Q 路点候选选择器节点。
架构概览
--------
本 launch 文件包含两个核心组件：
1. obstacle_nav2 : 完整的障碍物导航栈（底盘驱动、激光雷达、Nav2 导航、障碍物扫描）
2. q_waypoint_selector : 独立的 Python 节点，周期性从 costmap 中评估并选择
最优的 Q 路点候选目标，通过 NavigateToPose action 下发导航目标。
Q 路点选择器的设计意图
----------------------
在比赛中，我们需要依次访问一系列路点（Q₁, Q₂, ..., Qₙ），但每个路点可能有多个
候选位置。选择器从 JSON 文件中加载有序路点及其候选坐标，结合全局 costmap 的
占用栅格信息，过滤掉被障碍物占据的候选点，并向 Nav2 下发当前最优的导航目标。
启动方式
--------
ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \
use_sim_time:=false \
global_frame:=odom \
json_dir:=/path/to/json \
goal_frame:=odom \
costmap_topic:=/global_costmap/costmap
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.substitutions import FindExecutable
def generate_launch_description():
# =========================================================================
# 路径配置 —— 定位 obstacle_nav2 包内的关键文件
# =========================================================================
# obstacle_nav2 包的共享目录（install 后的 share/obstacle_nav2）
pkg_dir = get_package_share_directory("obstacle_nav2")
# 基础导航 launch 文件路径，将被 IncludeLaunchDescription 引入
base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py")
# Q 路点选择器的 Python 脚本路径，将被 ExecuteProcess 作为独立进程运行
selector_script = os.path.join(pkg_dir, "config", "q_waypoint_selector.py")
# 默认的 JSON 路点文件目录
default_json_dir = os.path.join(pkg_dir, "config", "json")
# =========================================================================
# LaunchConfiguration —— 运行时参数占位符
# 这些变量在 launch 时被替换为实际的参数值（命令行传入或默认值）。
# 使用 LaunchConfiguration 而非直接读取值，是为了支持 launch 系统的
# 延迟求值（lazy evaluation）机制。
# =========================================================================
# ---- obstacle_nav2 基础导航栈参数 ----
# 是否使用仿真时间（实车=false，Gazebo 仿真=true）
use_sim_time = LaunchConfiguration("use_sim_time")
# 全局坐标系名称（仿真用 "odom"，实车用 "odom_combined"）
global_frame = LaunchConfiguration("global_frame")
# 是否加载静态地图（已知地图导航=true，纯 SLAM 探索=false）
use_static_map = LaunchConfiguration("use_static_map")
# 静态地图 YAML 文件路径（为空时不加载 map_server）
map_yaml = LaunchConfiguration("map_yaml")
# 是否启用底盘运动控制（调试时可关闭以只运行感知）
enable_motion = LaunchConfiguration("enable_motion")
# 是否启动底盘驱动节点（实车=true，纯仿真可视化=false）
start_base = LaunchConfiguration("start_base")
# 是否启动激光雷达驱动节点
start_lidar = LaunchConfiguration("start_lidar")
# 是否启动障碍物扫描节点（obstacle_scanner）
start_obstacle_scanner = LaunchConfiguration("start_obstacle_scanner")
# ---- Q 路点选择器专属参数 ----
# JSON 路点文件所在目录，包含如 Q1.json, Q2.json 等有序路点定义
json_dir = LaunchConfiguration("json_dir")
# 下发 NavigateToPose 导航目标时使用的坐标系
goal_frame = LaunchConfiguration("goal_frame")
# 全局 costmap 话题，用于判断候选路点是否被障碍物占据
costmap_topic = LaunchConfiguration("costmap_topic")
# Nav2 的 NavigateToPose action 名称
navigate_action = LaunchConfiguration("navigate_action")
# 占用阈值：costmap 栅格值 >= 此值的单元格视为被占用（范围 0-100）
occupied_threshold = LaunchConfiguration("occupied_threshold")
# 是否将未知区域（costmap 值 == -1）视为占用
treat_unknown_as_occupied = LaunchConfiguration("treat_unknown_as_occupied")
# 单个候选目标的超时时间（秒），超时后取消当前目标并尝试下一个候选
goal_timeout_sec = LaunchConfiguration("goal_timeout_sec")
# 选择器的控制周期（秒），即多久重新评估一次最优候选路点
selection_period_sec = LaunchConfiguration("selection_period_sec")
# =========================================================================
# 组件 1 —— obstacle_nav2 基础导航栈
# 通过 IncludeLaunchDescription 引入 obstacle_nav2.launch.py，
# 将其作为一个子 launch 嵌入当前 launch 描述中。
# 传入的参数会透传给子 launch 文件，实现参数的统一管理。
# =========================================================================
obstacle_nav2 = IncludeLaunchDescription(
PythonLaunchDescriptionSource(base_launch),
launch_arguments={
"use_sim_time": use_sim_time,
"global_frame": global_frame,
"use_static_map": use_static_map,
"map_yaml": map_yaml,
"enable_motion": enable_motion,
"start_base": start_base,
"start_lidar": start_lidar,
"start_obstacle_scanner": start_obstacle_scanner,
}.items(),
)
# =========================================================================
# 组件 2 —— Q 路点候选选择器
# 作为一个独立的 Python 进程运行（ExecuteProcess），而非 ROS 2 Node action。
# 选择器内部会创建一个 rclpy 节点，订阅 costmap、发布导航目标。
# 参数通过 ROS 2 标准的 --ros-args -p 方式传递。
# =========================================================================
q_waypoint_selector = ExecuteProcess(
cmd=[
# 使用 FindExecutable 查找 python3 可执行文件，确保跨平台兼容
FindExecutable(name="python3"),
selector_script,
# ROS 2 命令行参数传递标准格式
"--ros-args",
"-p", ["json_dir:=", json_dir],
"-p", ["goal_frame:=", goal_frame],
"-p", ["costmap_topic:=", costmap_topic],
"-p", ["navigate_action:=", navigate_action],
"-p", ["occupied_threshold:=", occupied_threshold],
"-p", ["treat_unknown_as_occupied:=", treat_unknown_as_occupied],
"-p", ["goal_timeout_sec:=", goal_timeout_sec],
"-p", ["selection_period_sec:=", selection_period_sec],
"-p", ["use_sim_time:=", use_sim_time],
],
# 将进程的标准输出打印到终端，便于调试
output="screen",
emulate_tty=True,
)
# =========================================================================
# LaunchDescription —— 组装并返回完整的 launch 描述
# 包含三部分：
# 1. DeclareLaunchArgument —— 声明所有可配置参数及其默认值
# 2. obstacle_nav2 —— 基础导航栈
# 3. q_waypoint_selector —— Q 路点选择器
# =========================================================================
return LaunchDescription(
[
# =================================================================
# obstacle_nav2 基础导航栈参数声明
# =================================================================
# 是否使用仿真时间（/clock 话题），实车场景应设为 false
DeclareLaunchArgument("use_sim_time", default_value="false"),
# 全局坐标系：仿真用 "odom"，实车 EKF 融合后用 "odom_combined"
DeclareLaunchArgument("global_frame", default_value="odom"),
# 是否启用静态地图层（map_server），在线 SLAM 建图时设为 false
DeclareLaunchArgument("use_static_map", default_value="false"),
# 静态地图 yaml 文件路径，为空字符串时不加载地图
DeclareLaunchArgument("map_yaml", default_value=""),
# 是否启用底盘运动控制指令下发，调试时可关闭
DeclareLaunchArgument("enable_motion", default_value="true"),
# 是否启动底盘串口驱动节点（实车需要，仿真不需要）
DeclareLaunchArgument("start_base", default_value="true"),
# 是否启动激光雷达驱动节点
DeclareLaunchArgument("start_lidar", default_value="true"),
# 是否启动障碍物扫描与避障节点
DeclareLaunchArgument("start_obstacle_scanner", default_value="true"),
# =================================================================
# Q 路点选择器参数声明
# =================================================================
# JSON 路点文件目录，每个 JSON 文件定义一个路点的候选坐标列表
DeclareLaunchArgument(
"json_dir",
default_value=default_json_dir,
description="Directory containing ordered waypoint JSON files.",
),
# 导航目标使用的坐标帧，应与全局 costmap 的坐标系一致
DeclareLaunchArgument(
"goal_frame",
default_value="odom",
description="Frame used for NavigateToPose goals.",
),
# 全局 costmap 话题，选择器订阅此话题来评估候选点的可通行性
DeclareLaunchArgument(
"costmap_topic",
default_value="/global_costmap/costmap",
description="OccupancyGrid topic used to reject occupied candidates.",
),
# NavigateToPose action 的服务名，与 Nav2 的 bt_navigator 对应
DeclareLaunchArgument(
"navigate_action",
default_value="/navigate_to_pose",
description="Nav2 NavigateToPose action name.",
),
# costmap 占用判定阈值（0-100），>= 此值的栅格视为障碍物
DeclareLaunchArgument(
"occupied_threshold",
default_value="50",
description="Costmap cells >= this value are considered occupied.",
),
# 是否将未知区域（costmap 值 == -1）视为不可通行
DeclareLaunchArgument(
"treat_unknown_as_occupied",
default_value="true",
description="Treat costmap value -1 as occupied.",
),
# 单个导航目标的超时时间，超时后自动取消并切换候选
DeclareLaunchArgument(
"goal_timeout_sec",
default_value="60.0",
description="Cancel a candidate goal if it does not finish before this timeout.",
),
# 选择器主循环的控制周期（秒），值越小响应越快但 CPU 开销越大
DeclareLaunchArgument(
"selection_period_sec",
default_value="0.5",
description="Selector control period.",
),
# ---- 组装已配置的 action ----
obstacle_nav2,
q_waypoint_selector,
]
)

277
src/navigation/obstacle_nav2/src/trajectory_guard.cpp Normal file
View File

@@ -0,0 +1,277 @@
#include "obstacle_nav2/trajectory_guard.hpp"
#include <algorithm>
#include <cmath>
#include <limits>
namespace obstacle_nav2
{
namespace
{
bool worldToMap(
const nav_msgs::msg::OccupancyGrid & grid,
double wx,
double wy,
int & mx,
int & my)
{
const double resolution = grid.info.resolution;
if (resolution <= 0.0) {
return false;
}
const double ox = grid.info.origin.position.x;
const double oy = grid.info.origin.position.y;
if (wx < ox || wy < oy) {
return false;
}
mx = static_cast<int>((wx - ox) / resolution);
my = static_cast<int>((wy - oy) / resolution);
return mx >= 0 && my >= 0 &&
mx < static_cast<int>(grid.info.width) &&
my < static_cast<int>(grid.info.height);
}
bool occupiedAt(
const nav_msgs::msg::OccupancyGrid & grid,
double wx,
double wy,
const GuardSettings & settings)
{
int mx = 0;
int my = 0;
if (!worldToMap(grid, wx, wy, mx, my)) {
return settings.treat_unknown_as_occupied;
}
const auto value = grid.data[my * grid.info.width + mx];
if (value < 0) {
return settings.treat_unknown_as_occupied;
}
return value >= settings.occupied_threshold;
}
bool footprintOccupied(
const nav_msgs::msg::OccupancyGrid & costmap,
double cx,
double cy,
double yaw,
const GuardSettings & settings)
{
const double half_l = settings.footprint_half_length + settings.footprint_padding;
const double half_w = settings.footprint_half_width + settings.footprint_padding;
const double step = std::max(0.02, settings.footprint_sample_step);
const double c = std::cos(yaw);
const double s = std::sin(yaw);
auto sample = [&](double x, double y) {
const double wx = cx + c * x - s * y;
const double wy = cy + s * x + c * y;
return occupiedAt(costmap, wx, wy, settings);
};
if (sample(0.0, 0.0)) {
return true;
}
for (double x = -half_l; x <= half_l + 1.0e-9; x += step) {
if (sample(x, -half_w) || sample(x, 0.0) || sample(x, half_w)) {
return true;
}
}
for (double y = -half_w; y <= half_w + 1.0e-9; y += step) {
if (sample(-half_l, y) || sample(0.0, y) || sample(half_l, y)) {
return true;
}
}
return false;
}
} // namespace
double yawFromPose(const geometry_msgs::msg::PoseStamped & pose)
{
const auto & q = pose.pose.orientation;
return std::atan2(
2.0 * (q.w * q.z + q.x * q.y),
1.0 - 2.0 * (q.y * q.y + q.z * q.z));
}
double distance2d(
const geometry_msgs::msg::PoseStamped & a,
const geometry_msgs::msg::PoseStamped & b)
{
const double dx = a.pose.position.x - b.pose.position.x;
const double dy = a.pose.position.y - b.pose.position.y;
return std::hypot(dx, dy);
}
std::size_t nearestPathIndex(
const nav_msgs::msg::Path & path,
const geometry_msgs::msg::PoseStamped & pose)
{
std::size_t best_index = 0;
double best_distance = std::numeric_limits<double>::infinity();
for (std::size_t i = 0; i < path.poses.size(); ++i) {
const double d = distance2d(path.poses[i], pose);
if (d < best_distance) {
best_distance = d;
best_index = i;
}
}
return best_index;
}
std::size_t advanceByDistance(
const nav_msgs::msg::Path & path,
std::size_t start_index,
double distance_m)
{
if (path.poses.empty()) {
return 0;
}
std::size_t index = std::min(start_index, path.poses.size() - 1);
double traveled = 0.0;
while (index + 1 < path.poses.size() && traveled < distance_m) {
traveled += distance2d(path.poses[index], path.poses[index + 1]);
++index;
}
return index;
}
CollisionCheckResult checkPathAhead(
const nav_msgs::msg::Path & path,
std::size_t start_index,
const nav_msgs::msg::OccupancyGrid & costmap,
const GuardSettings & settings)
{
if (path.poses.empty()) {
return CollisionCheckResult{true, 0, "empty_path"};
}
const auto clamped_start = std::min(start_index, path.poses.size() - 1);
const auto end_index =
advanceByDistance(path, clamped_start, settings.lookahead_distance);
const double step = std::max(0.02, settings.footprint_sample_step);
for (std::size_t i = clamped_start; i <= end_index && i < path.poses.size(); ++i) {
if (i + 1 >= path.poses.size() || i == end_index) {
const auto & pose = path.poses[i];
if (footprintOccupied(
costmap, pose.pose.position.x, pose.pose.position.y, yawFromPose(pose), settings))
{
return CollisionCheckResult{true, i, "occupied"};
}
continue;
}
const auto & a = path.poses[i];
const auto & b = path.poses[i + 1];
const double dx = b.pose.position.x - a.pose.position.x;
const double dy = b.pose.position.y - a.pose.position.y;
const double segment_length = std::hypot(dx, dy);
const double yaw = segment_length > 1.0e-6 ? std::atan2(dy, dx) : yawFromPose(a);
const int samples = std::max(1, static_cast<int>(std::ceil(segment_length / step)));
for (int sample_index = 0; sample_index <= samples; ++sample_index) {
const double t = static_cast<double>(sample_index) / static_cast<double>(samples);
const double x = a.pose.position.x + t * dx;
const double y = a.pose.position.y + t * dy;
if (footprintOccupied(costmap, x, y, yaw, settings)) {
return CollisionCheckResult{true, i, "occupied"};
}
}
}
return CollisionCheckResult{false, end_index, "clear"};
}
std::optional<std::size_t> findClearRejoinIndex(
const nav_msgs::msg::Path & path,
std::size_t start_index,
const nav_msgs::msg::OccupancyGrid & costmap,
const GuardSettings & settings)
{
if (path.poses.empty()) {
return std::nullopt;
}
const auto first = advanceByDistance(path, start_index, settings.rejoin_min_distance);
const auto last = advanceByDistance(path, start_index, settings.rejoin_max_distance);
GuardSettings single_pose_settings = settings;
single_pose_settings.lookahead_distance = 0.0;
for (std::size_t i = first; i <= last && i < path.poses.size(); ++i) {
const auto result = checkPathAhead(path, i, costmap, single_pose_settings);
if (!result.blocked) {
return i;
}
}
return std::nullopt;
}
bool shouldRetryBlockedRepair(
const std::optional<std::size_t> & last_repair_nearest_index,
std::size_t nearest_index,
int min_repair_progress_indices,
double seconds_since_last_repair,
double blocked_retry_wait_sec,
bool costmap_updated_since_repair)
{
if (!last_repair_nearest_index.has_value()) {
return true;
}
const auto min_progress =
static_cast<std::size_t>(std::max(0, min_repair_progress_indices));
if (nearest_index > *last_repair_nearest_index + min_progress) {
return true;
}
return seconds_since_last_repair >= blocked_retry_wait_sec &&
costmap_updated_since_repair;
}
nav_msgs::msg::Path slicePath(
const nav_msgs::msg::Path & path,
std::size_t start_index,
std::size_t end_index_inclusive)
{
nav_msgs::msg::Path out;
out.header = path.header;
if (path.poses.empty()) {
return out;
}
const auto begin = std::min(start_index, path.poses.size() - 1);
const auto end = std::min(end_index_inclusive, path.poses.size() - 1);
for (std::size_t i = begin; i <= end; ++i) {
out.poses.push_back(path.poses[i]);
}
return out;
}
nav_msgs::msg::Path stitchPaths(
const nav_msgs::msg::Path & bypass,
const nav_msgs::msg::Path & original,
std::size_t rejoin_index)
{
nav_msgs::msg::Path out;
out.header = bypass.header.frame_id.empty() ? original.header : bypass.header;
out.poses = bypass.poses;
if (original.poses.empty()) {
return out;
}
const auto begin = std::min(rejoin_index, original.poses.size());
for (std::size_t i = begin; i < original.poses.size(); ++i) {
out.poses.push_back(original.poses[i]);
}
return out;
}
} // namespace obstacle_nav2

454
src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp Normal file
View File

@@ -0,0 +1,454 @@
#include <algorithm>
#include <chrono>
#include <cstdint>
#include <memory>
#include <optional>
#include <string>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav2_msgs/action/compute_path_to_pose.hpp"
#include "nav2_msgs/action/follow_path.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "obstacle_nav2/trajectory_guard.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
using namespace std::chrono_literals;
namespace obstacle_nav2
{
class TrajectoryGuardNode : public rclcpp::Node
{
public:
using ComputePathToPose = nav2_msgs::action::ComputePathToPose;
using FollowPath = nav2_msgs::action::FollowPath;
using ComputeGoalHandle = rclcpp_action::ClientGoalHandle<ComputePathToPose>;
using FollowGoalHandle = rclcpp_action::ClientGoalHandle<FollowPath>;
TrajectoryGuardNode()
: Node("trajectory_guard_node")
{
declare_parameter("input_path_topic", "/trajectory_guard/input_path");
declare_parameter("patched_path_topic", "/trajectory_guard/patched_path");
declare_parameter("costmap_topic", "/local_costmap/costmap");
declare_parameter("planner_costmap_topic", "/global_costmap/costmap");
declare_parameter("odom_topic", "/odom_combined");
declare_parameter("planner_action", "/compute_path_to_pose");
declare_parameter("follow_action", "/follow_path");
declare_parameter("planner_id", "GridBased");
declare_parameter("controller_id", "FollowPath");
declare_parameter("goal_checker_id", "");
declare_parameter("check_period_sec", 0.5);
declare_parameter("lookahead_distance", 2.0);
declare_parameter("rejoin_min_distance", 1.0);
declare_parameter("rejoin_max_distance", 4.0);
declare_parameter("occupied_threshold", 50);
declare_parameter("treat_unknown_as_occupied", true);
declare_parameter("footprint_half_length", 0.14);
declare_parameter("footprint_half_width", 0.085);
declare_parameter("footprint_padding", 0.04);
declare_parameter("footprint_sample_step", 0.05);
declare_parameter("repair_cooldown_sec", 1.0);
declare_parameter("blocked_retry_wait_sec", 0.8);
declare_parameter("wait_for_planner_costmap_update", true);
declare_parameter("min_repair_progress_indices", 5);
declare_parameter("publish_zero_on_blocked", true);
declare_parameter("cmd_vel_topic", "/cmd_vel");
declare_parameter("execute_follow_path", true);
loadParameters();
last_repair_request_time_ =
get_clock()->now() - rclcpp::Duration::from_seconds(repair_cooldown_sec_);
path_sub_ = create_subscription<nav_msgs::msg::Path>(
get_parameter("input_path_topic").as_string(), 1,
[this](nav_msgs::msg::Path::SharedPtr msg) { onPath(msg); });
costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
get_parameter("costmap_topic").as_string(), 1,
[this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
latest_costmap_ = msg;
++local_costmap_generation_;
});
planner_costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
get_parameter("planner_costmap_topic").as_string(), 1,
[this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
latest_planner_costmap_ = msg;
++planner_costmap_generation_;
});
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
get_parameter("odom_topic").as_string(), 10,
[this](nav_msgs::msg::Odometry::SharedPtr msg) { latest_odom_ = msg; });
patched_path_pub_ = create_publisher<nav_msgs::msg::Path>(
get_parameter("patched_path_topic").as_string(), 1);
cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
get_parameter("cmd_vel_topic").as_string(), 1);
planner_client_ = rclcpp_action::create_client<ComputePathToPose>(
this, get_parameter("planner_action").as_string());
follow_client_ = rclcpp_action::create_client<FollowPath>(
this, get_parameter("follow_action").as_string());
const auto period = std::chrono::duration<double>(
std::max(0.1, get_parameter("check_period_sec").as_double()));
timer_ = create_wall_timer(
std::chrono::duration_cast<std::chrono::milliseconds>(period),
[this]() { tick(); });
RCLCPP_INFO(
get_logger(), "trajectory guard ready check_period_sec=%.2f input=%s patched=%s",
get_parameter("check_period_sec").as_double(),
get_parameter("input_path_topic").as_string().c_str(),
get_parameter("patched_path_topic").as_string().c_str());
}
private:
void loadParameters()
{
settings_.lookahead_distance = get_parameter("lookahead_distance").as_double();
settings_.rejoin_min_distance = get_parameter("rejoin_min_distance").as_double();
settings_.rejoin_max_distance = get_parameter("rejoin_max_distance").as_double();
settings_.occupied_threshold = get_parameter("occupied_threshold").as_int();
settings_.treat_unknown_as_occupied =
get_parameter("treat_unknown_as_occupied").as_bool();
settings_.footprint_half_length = get_parameter("footprint_half_length").as_double();
settings_.footprint_half_width = get_parameter("footprint_half_width").as_double();
settings_.footprint_padding = get_parameter("footprint_padding").as_double();
settings_.footprint_sample_step = get_parameter("footprint_sample_step").as_double();
repair_cooldown_sec_ = std::max(0.0, get_parameter("repair_cooldown_sec").as_double());
blocked_retry_wait_sec_ =
std::max(0.0, get_parameter("blocked_retry_wait_sec").as_double());
wait_for_planner_costmap_update_ =
get_parameter("wait_for_planner_costmap_update").as_bool();
min_repair_progress_indices_ =
std::max(0, static_cast<int>(get_parameter("min_repair_progress_indices").as_int()));
publish_zero_on_blocked_ = get_parameter("publish_zero_on_blocked").as_bool();
execute_follow_path_ = get_parameter("execute_follow_path").as_bool();
}
void onPath(const nav_msgs::msg::Path::SharedPtr msg)
{
active_path_ = *msg;
sent_original_for_current_path_ = false;
last_repair_nearest_index_.reset();
last_repair_rejoin_index_.reset();
retry_without_planner_costmap_update_ = false;
waiting_for_planner_costmap_update_ = false;
RCLCPP_INFO(
get_logger(), "received replay path poses=%zu frame=%s",
active_path_.poses.size(), active_path_.header.frame_id.c_str());
}
std::optional<geometry_msgs::msg::PoseStamped> currentPose() const
{
if (!latest_odom_) {
return std::nullopt;
}
geometry_msgs::msg::PoseStamped pose;
pose.header = latest_odom_->header;
pose.pose = latest_odom_->pose.pose;
return pose;
}
void tick()
{
if (active_path_.poses.empty() || !latest_costmap_) {
return;
}
const auto pose = currentPose();
if (!pose.has_value()) {
return;
}
const auto nearest = nearestPathIndex(active_path_, *pose);
const auto result = checkPathAhead(active_path_, nearest, *latest_costmap_, settings_);
if (!result.blocked) {
if (!sent_original_for_current_path_) {
publishAndMaybeFollow(active_path_, "original_clear");
sent_original_for_current_path_ = true;
}
return;
}
const auto now = get_clock()->now();
if (repair_in_flight_ ||
(now - last_repair_request_time_).seconds() < repair_cooldown_sec_)
{
return;
}
if (!waiting_for_planner_costmap_update_) {
waiting_for_planner_costmap_update_ = true;
waiting_planner_costmap_generation_ = planner_costmap_generation_;
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 2000,
"blocked replay path; waiting for next planner costmap frame before requesting bypass");
publishStop();
return;
}
if (planner_costmap_generation_ <= waiting_planner_costmap_generation_) {
if (retry_without_planner_costmap_update_ &&
(now - last_repair_request_time_).seconds() >= blocked_retry_wait_sec_)
{
waiting_for_planner_costmap_update_ = false;
} else {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 2000,
"blocked replay path; planner costmap has not updated yet");
publishStop();
return;
}
}
waiting_for_planner_costmap_update_ = false;
const bool costmap_updated_since_repair =
!wait_for_planner_costmap_update_ ||
planner_costmap_generation_ > last_repair_planner_costmap_generation_;
const bool timeout_retry_allowed =
retry_without_planner_costmap_update_ &&
(now - last_repair_request_time_).seconds() >= blocked_retry_wait_sec_;
if (!shouldRetryBlockedRepair(
last_repair_nearest_index_, nearest, min_repair_progress_indices_,
(now - last_repair_request_time_).seconds(), blocked_retry_wait_sec_,
costmap_updated_since_repair) && !timeout_retry_allowed)
{
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 2000,
"patched path still blocked near the last repair point; waiting for path progress or costmap update");
publishStop();
return;
}
retry_without_planner_costmap_update_ = false;
std::size_t search_start = nearest;
if (last_repair_rejoin_index_.has_value()) {
const auto bump = static_cast<std::size_t>(std::max(1, min_repair_progress_indices_));
const auto advanced = std::min(
active_path_.poses.size() - 1, *last_repair_rejoin_index_ + bump);
search_start = std::max(search_start, advanced);
}
const auto rejoin = findClearRejoinIndex(
active_path_, search_start, *latest_costmap_, settings_);
if (!rejoin.has_value()) {
RCLCPP_WARN(get_logger(), "blocked replay path but no clear rejoin point found");
publishStop();
return;
}
RCLCPP_WARN(
get_logger(), "blocked replay path index=%zu reason=%s rejoin_index=%zu",
result.path_index, result.reason.c_str(), *rejoin);
requestBypass(nearest, *rejoin);
}
void publishAndMaybeFollow(const nav_msgs::msg::Path & path, const std::string & label)
{
if (path.poses.empty()) {
return;
}
nav_msgs::msg::Path stamped_path = path;
stamped_path.header.stamp = now();
for (auto & pose : stamped_path.poses) {
pose.header.stamp = stamped_path.header.stamp;
if (pose.header.frame_id.empty()) {
pose.header.frame_id = stamped_path.header.frame_id;
}
}
patched_path_pub_->publish(stamped_path);
if (!execute_follow_path_) {
RCLCPP_INFO(
get_logger(), "published %s path poses=%zu without FollowPath execution",
label.c_str(), stamped_path.poses.size());
return;
}
sendFollowPath(stamped_path, label);
}
void sendFollowPath(const nav_msgs::msg::Path & path, const std::string & label)
{
if (!follow_client_->wait_for_action_server(100ms)) {
RCLCPP_WARN(get_logger(), "FollowPath action server not ready");
return;
}
if (active_follow_goal_handle_) {
follow_client_->async_cancel_goal(active_follow_goal_handle_);
active_follow_goal_handle_.reset();
}
FollowPath::Goal goal;
goal.path = path;
goal.controller_id = get_parameter("controller_id").as_string();
goal.goal_checker_id = get_parameter("goal_checker_id").as_string();
auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
options.goal_response_callback =
[this, label](FollowGoalHandle::SharedPtr goal_handle) {
if (!goal_handle) {
RCLCPP_WARN(get_logger(), "FollowPath %s rejected", label.c_str());
return;
}
active_follow_goal_handle_ = goal_handle;
RCLCPP_INFO(get_logger(), "FollowPath %s accepted", label.c_str());
};
options.result_callback =
[this, label](const FollowGoalHandle::WrappedResult & result) {
active_follow_goal_handle_.reset();
RCLCPP_INFO(
get_logger(), "FollowPath %s finished code=%d",
label.c_str(), static_cast<int>(result.code));
};
follow_client_->async_send_goal(goal, options);
RCLCPP_INFO(get_logger(), "sent FollowPath %s poses=%zu", label.c_str(), path.poses.size());
}
void requestBypass(std::size_t nearest_index, std::size_t rejoin_index)
{
const auto start = currentPose();
if (!start.has_value() || rejoin_index >= active_path_.poses.size()) {
return;
}
if (!planner_client_->wait_for_action_server(100ms)) {
RCLCPP_WARN(get_logger(), "ComputePathToPose action server not ready");
publishStop();
return;
}
repair_in_flight_ = true;
last_repair_request_time_ = get_clock()->now();
last_repair_nearest_index_ = nearest_index;
last_repair_rejoin_index_ = rejoin_index;
last_repair_planner_costmap_generation_ = planner_costmap_generation_;
waiting_for_planner_costmap_update_ = false;
ComputePathToPose::Goal goal;
goal.start = *start;
goal.goal = active_path_.poses[rejoin_index];
goal.planner_id = get_parameter("planner_id").as_string();
goal.use_start = true;
auto options = rclcpp_action::Client<ComputePathToPose>::SendGoalOptions();
options.goal_response_callback =
[this](ComputeGoalHandle::SharedPtr goal_handle) {
if (!goal_handle) {
repair_in_flight_ = false;
RCLCPP_WARN(get_logger(), "bypass planning goal rejected");
}
};
options.result_callback =
[this, rejoin_index](const ComputeGoalHandle::WrappedResult & result) {
repair_in_flight_ = false;
if (result.code != rclcpp_action::ResultCode::SUCCEEDED ||
result.result->path.poses.empty())
{
RCLCPP_WARN(get_logger(), "bypass planning failed code=%d", static_cast<int>(result.code));
publishStop();
return;
}
const auto patched = stitchPaths(result.result->path, active_path_, rejoin_index);
if (!patchedPathIsClear(patched)) {
retry_without_planner_costmap_update_ = true;
waiting_for_planner_costmap_update_ = false;
RCLCPP_WARN(
get_logger(),
"planned bypass still blocked in local costmap; stopping and retrying from a later rejoin point");
publishStop();
return;
}
active_path_ = patched;
sent_original_for_current_path_ = true;
waiting_for_planner_costmap_update_ = false;
retry_without_planner_costmap_update_ = false;
publishAndMaybeFollow(patched, "patched_bypass");
};
planner_client_->async_send_goal(goal, options);
RCLCPP_INFO(get_logger(), "requested bypass to rejoin_index=%zu", rejoin_index);
}
bool patchedPathIsClear(const nav_msgs::msg::Path & patched) const
{
if (!latest_costmap_) {
return false;
}
const auto pose = currentPose();
if (!pose.has_value()) {
return false;
}
const auto nearest = nearestPathIndex(patched, *pose);
const auto result = checkPathAhead(patched, nearest, *latest_costmap_, settings_);
return !result.blocked;
}
void publishStop()
{
if (!publish_zero_on_blocked_) {
return;
}
geometry_msgs::msg::Twist stop;
cmd_vel_pub_->publish(stop);
}
GuardSettings settings_;
double repair_cooldown_sec_{1.0};
double blocked_retry_wait_sec_{0.8};
int min_repair_progress_indices_{5};
bool publish_zero_on_blocked_{true};
bool execute_follow_path_{true};
bool wait_for_planner_costmap_update_{true};
bool retry_without_planner_costmap_update_{false};
bool repair_in_flight_{false};
bool sent_original_for_current_path_{false};
bool waiting_for_planner_costmap_update_{false};
rclcpp::Time last_repair_request_time_{0, 0, RCL_ROS_TIME};
std::optional<std::size_t> last_repair_nearest_index_;
std::optional<std::size_t> last_repair_rejoin_index_;
std::uint64_t local_costmap_generation_{0};
std::uint64_t planner_costmap_generation_{0};
std::uint64_t last_repair_planner_costmap_generation_{0};
std::uint64_t waiting_planner_costmap_generation_{0};
nav_msgs::msg::Path active_path_;
nav_msgs::msg::OccupancyGrid::SharedPtr latest_costmap_;
nav_msgs::msg::OccupancyGrid::SharedPtr latest_planner_costmap_;
nav_msgs::msg::Odometry::SharedPtr latest_odom_;
FollowGoalHandle::SharedPtr active_follow_goal_handle_;
rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_sub_;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr planner_costmap_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr patched_path_pub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
rclcpp_action::Client<ComputePathToPose>::SharedPtr planner_client_;
rclcpp_action::Client<FollowPath>::SharedPtr follow_client_;
rclcpp::TimerBase::SharedPtr timer_;
};
} // namespace obstacle_nav2
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<obstacle_nav2::TrajectoryGuardNode>());
rclcpp::shutdown();
return 0;
}

162
src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp Normal file
View File

@@ -0,0 +1,162 @@
#include <gtest/gtest.h>
#include "obstacle_nav2/trajectory_guard.hpp"
namespace
{
geometry_msgs::msg::PoseStamped pose(double x, double y)
{
geometry_msgs::msg::PoseStamped p;
p.header.frame_id = "odom";
p.pose.position.x = x;
p.pose.position.y = y;
p.pose.orientation.w = 1.0;
return p;
}
nav_msgs::msg::Path straightPath()
{
nav_msgs::msg::Path path;
path.header.frame_id = "odom";
for (int i = 0; i <= 10; ++i) {
path.poses.push_back(pose(0.2 * i, 0.0));
}
return path;
}
nav_msgs::msg::OccupancyGrid gridWithOrigin(double origin_x, double origin_y)
{
nav_msgs::msg::OccupancyGrid grid;
grid.header.frame_id = "odom";
grid.info.resolution = 0.1;
grid.info.width = 40;
grid.info.height = 30;
grid.info.origin.position.x = origin_x;
grid.info.origin.position.y = origin_y;
grid.data.assign(grid.info.width * grid.info.height, 0);
return grid;
}
void markOccupied(nav_msgs::msg::OccupancyGrid & grid, int mx, int my)
{
ASSERT_GE(mx, 0);
ASSERT_GE(my, 0);
ASSERT_LT(mx, static_cast<int>(grid.info.width));
ASSERT_LT(my, static_cast<int>(grid.info.height));
grid.data[my * grid.info.width + mx] = 100;
}
} // namespace
TEST(TrajectoryGuard, NearestPathIndexFindsClosestPose)
{
const auto path = straightPath();
EXPECT_EQ(obstacle_nav2::nearestPathIndex(path, pose(0.43, 0.02)), 2u);
}
TEST(TrajectoryGuard, AdvanceByDistanceStopsAtRequestedArcLength)
{
const auto path = straightPath();
EXPECT_EQ(obstacle_nav2::advanceByDistance(path, 0, 0.55), 3u);
}
TEST(TrajectoryGuard, SlicePathIncludesEndIndex)
{
const auto path = straightPath();
const auto sliced = obstacle_nav2::slicePath(path, 2, 4);
ASSERT_EQ(sliced.poses.size(), 3u);
EXPECT_DOUBLE_EQ(sliced.poses.front().pose.position.x, 0.4);
EXPECT_DOUBLE_EQ(sliced.poses.back().pose.position.x, 0.8);
}
TEST(TrajectoryGuard, StitchPathsAppendsOriginalFromRejoinIndex)
{
const auto original = straightPath();
nav_msgs::msg::Path bypass;
bypass.header.frame_id = "odom";
bypass.poses.push_back(pose(0.0, 0.0));
bypass.poses.push_back(pose(0.5, 0.3));
const auto stitched = obstacle_nav2::stitchPaths(bypass, original, 4);
ASSERT_EQ(stitched.poses.size(), 9u);
EXPECT_DOUBLE_EQ(stitched.poses[0].pose.position.y, 0.0);
EXPECT_DOUBLE_EQ(stitched.poses[1].pose.position.y, 0.3);
EXPECT_DOUBLE_EQ(stitched.poses[2].pose.position.x, 0.8);
}
TEST(TrajectoryGuard, CheckPathAheadReportsOccupiedCell)
{
auto path = straightPath();
auto grid = gridWithOrigin(-1.0, -1.0);
markOccupied(grid, 15, 10);
obstacle_nav2::GuardSettings settings;
settings.lookahead_distance = 2.0;
settings.occupied_threshold = 50;
settings.footprint_half_length = 0.01;
settings.footprint_half_width = 0.01;
settings.footprint_padding = 0.0;
const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
EXPECT_TRUE(result.blocked);
EXPECT_EQ(result.reason, "occupied");
}
TEST(TrajectoryGuard, CheckPathAheadKeepsClearPathUnblocked)
{
auto path = straightPath();
auto grid = gridWithOrigin(-1.0, -1.0);
obstacle_nav2::GuardSettings settings;
settings.lookahead_distance = 2.0;
settings.occupied_threshold = 50;
settings.footprint_half_length = 0.01;
settings.footprint_half_width = 0.01;
settings.footprint_padding = 0.0;
const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
EXPECT_FALSE(result.blocked);
EXPECT_EQ(result.reason, "clear");
}
TEST(TrajectoryGuard, FindClearRejoinIndexSkipsBlockedArea)
{
auto path = straightPath();
auto grid = gridWithOrigin(-1.0, -1.0);
for (int mx = 13; mx <= 18; ++mx) {
markOccupied(grid, mx, 10);
}
obstacle_nav2::GuardSettings settings;
settings.rejoin_min_distance = 0.5;
settings.rejoin_max_distance = 2.0;
settings.occupied_threshold = 50;
settings.footprint_half_length = 0.01;
settings.footprint_half_width = 0.01;
settings.footprint_padding = 0.0;
const auto rejoin = obstacle_nav2::findClearRejoinIndex(path, 0, grid, settings);
ASSERT_TRUE(rejoin.has_value());
EXPECT_GE(*rejoin, 5u);
}
TEST(TrajectoryGuard, RetryBlockedRepairWaitsForCostmapUpdateAndRetryDelay)
{
const std::optional<std::size_t> last_repair_index = 10u;
EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
last_repair_index, 11u, 5, 0.8, 1.0, true));
EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
last_repair_index, 11u, 5, 1.2, 1.0, false));
EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
last_repair_index, 11u, 5, 1.2, 1.0, true));
}
TEST(TrajectoryGuard, RetryBlockedRepairAllowsProgressedPathImmediately)
{
const std::optional<std::size_t> last_repair_index = 10u;
EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
last_repair_index, 16u, 5, 0.1, 1.0, false));
}