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

src/navigation/obstacle_nav2/config/nav2_params.yaml

@@ -112,8 +112,8 @@ controller_server:
       CostCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 4.5
+        cost_weight: 6.0
-        critical_cost: 300.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
+      update_frequency: 5.0
-      publish_frequency: 1.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
+      change_penalty: 0.0
-      non_straight_penalty: 0.85
+      non_straight_penalty: 0.8
-      cost_penalty: 4.5
+      cost_penalty: 5.0
-      retrospective_penalty: 0.015
+      retrospective_penalty: 0.01
       lookup_table_size: 5.0
       cache_obstacle_heuristic: false
       viz_expansions: false

src/navigation/obstacle_nav2/config/nav2_profile_10.yaml

@@ -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

src/navigation/obstacle_nav2/config/nav2_profile_11.yaml

@@ -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

src/navigation/obstacle_nav2/config/trajectory_guard.yaml

@@ -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

src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py

@@ -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,

src/navigation/obstacle_nav2/launch/obstacle_nav2_q_waypoints.launch.py

@@ -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,
+        ]
+    )

src/navigation/obstacle_nav2/src/trajectory_guard.cpp

@@ -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

src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp

@@ -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;
+}

src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp

@@ -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));
+}