40~60s完赛

src/navigation/obstacle_nav2/config/nav2_profile_10.yaml

@@ -102,7 +102,7 @@ controller_server:
       GoalCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 6.0
+        cost_weight: 5.0
         threshold_to_consider: 1.4
       GoalAngleCritic:
         enabled: true
@@ -126,7 +126,7 @@ controller_server:
       PathAlignCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 8.0
+        cost_weight: 10.0
         max_path_occupancy_ratio: 0.05
         trajectory_point_step: 4
         threshold_to_consider: 0.5
@@ -224,8 +224,8 @@ global_costmap:
         extra_inflation: 0.02
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 2.0
-        inflation_radius: 0.35
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.30
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
@@ -252,10 +252,10 @@ planner_server:
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 3.0
       minimum_turning_radius: 0.40
-      reverse_penalty: 1.6
-      change_penalty: 2.0
+      reverse_penalty: 2.0
+      change_penalty: 1.0
       non_straight_penalty: 1.2
-      cost_penalty: 4.0
+      cost_penalty: 3.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.

src/racing_control/config/racing_control.yaml

@@ -5,7 +5,7 @@ racing_control:
     frame_id: odom
     use_post_qr_pose: false
     enable_vlm_image_relay: false
-    enable_dynamic_replanning: true
+    enable_dynamic_replanning: false
     enable_recovery: true
     vlm_image_input_topic: /image
     vlm_image_output_topic: /vlm_image

src/racing_control/include/racing_control/racing_control.hpp

@@ -3,6 +3,7 @@
 
 #include <cmath>
 #include <cctype>
+#include <cstddef>
 #include <stdexcept>
 #include <string>
 #include <vector>
@@ -153,6 +154,38 @@ inline geometry_msgs::msg::PoseStamped poseFromXYYaw(
   return pose;
 }
 
+inline double poseDistance2d(
+  const geometry_msgs::msg::PoseStamped & a,
+  const geometry_msgs::msg::PoseStamped & b)
+{
+  const auto dx = a.pose.position.x - b.pose.position.x;
+  const auto dy = a.pose.position.y - b.pose.position.y;
+  return std::hypot(dx, dy);
+}
+
+inline std::size_t advanceReachedWaypointIndex(
+  const std::vector<geometry_msgs::msg::PoseStamped> & waypoints,
+  const geometry_msgs::msg::PoseStamped & current,
+  const std::size_t next_waypoint_index,
+  const double tolerance)
+{
+  auto next = next_waypoint_index;
+  while (next < waypoints.size() && poseDistance2d(current, waypoints[next]) <= tolerance) {
+    ++next;
+  }
+  return next;
+}
+
+inline std::vector<geometry_msgs::msg::PoseStamped> remainingWaypoints(
+  const std::vector<geometry_msgs::msg::PoseStamped> & waypoints,
+  const std::size_t next_waypoint_index)
+{
+  if (next_waypoint_index >= waypoints.size()) {
+    return {};
+  }
+  return {waypoints.begin() + static_cast<std::ptrdiff_t>(next_waypoint_index), waypoints.end()};
+}
+
 inline std::vector<geometry_msgs::msg::PoseStamped> posesFromFlatDoubles(
   const std::vector<double> & values, const std::string & frame_id)
 {

src/racing_control/src/racing_control.cpp

@@ -616,6 +616,7 @@ private:
     if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) {
       active_segment_ = RouteSegment::FullRoute;
       active_segment_waypoints_ = route.waypoints;
+      active_segment_next_waypoint_index_ = 0;
       runRouteSegmentPlanning("full route with pass-through VLM capture");
       return;
     }
@@ -624,6 +625,7 @@ private:
     active_segment_waypoints_.assign(
       route.waypoints.begin(),
       route.waypoints.begin() + vlm_index + 1);
+    active_segment_next_waypoint_index_ = 0;
     runRouteSegmentPlanning("to VLM waypoint");
   }
 
@@ -640,6 +642,7 @@ private:
     active_segment_waypoints_.assign(
       route.waypoints.begin() + vlm_index + 1,
       route.waypoints.end());
+    active_segment_next_waypoint_index_ = 0;
     runRouteSegmentPlanning("after VLM waypoint");
   }
 
@@ -809,6 +812,10 @@ private:
     if (!current) {
       return;
     }
+    updateActiveSegmentWaypointProgress(*current);
+    if (active_segment_next_waypoint_index_ >= active_segment_waypoints_.size()) {
+      return;
+    }
     const auto distance_to_goal = distance2d(*current, active_segment_waypoints_.back());
     const auto elapsed = (now() - last_dynamic_replan_time_).seconds();
     if (!shouldDynamicReplan(
@@ -830,8 +837,15 @@ private:
       return;
     }
 
+    const auto replan_goals = remainingWaypoints(
+      active_segment_waypoints_, active_segment_next_waypoint_index_);
+    if (replan_goals.empty()) {
+      dynamic_replan_in_flight_ = false;
+      return;
+    }
+
     ComputePathThroughPoses::Goal goal;
-    goal.goals = stampPoses({active_segment_waypoints_.back()});
+    goal.goals = stampPoses(replan_goals);
     goal.planner_id = planner_id_;
     goal.use_start = false;
 
@@ -864,6 +878,22 @@ private:
       };
 
     compute_path_client_->async_send_goal(goal, options);
+    RCLCPP_INFO(
+      get_logger(), "dynamic route replanning through %zu remaining waypoint(s), next index=%zu",
+      replan_goals.size(), active_segment_next_waypoint_index_);
+  }
+
+  void updateActiveSegmentWaypointProgress(const geometry_msgs::msg::PoseStamped & current)
+  {
+    const auto previous_index = active_segment_next_waypoint_index_;
+    active_segment_next_waypoint_index_ = advanceReachedWaypointIndex(
+      active_segment_waypoints_, current, active_segment_next_waypoint_index_,
+      circle_goal_tolerance_);
+    if (active_segment_next_waypoint_index_ != previous_index) {
+      RCLCPP_INFO(
+        get_logger(), "route waypoint progress advanced: next index %zu -> %zu",
+        previous_index, active_segment_next_waypoint_index_);
+    }
   }
 
   void onDynamicReplanFailed(const std::string & reason)
@@ -1293,6 +1323,7 @@ private:
   VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop};
   RouteSegment active_segment_{RouteSegment::None};
   std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
+  std::size_t active_segment_next_waypoint_index_{0};
   nav_msgs::msg::Path active_path_;
   bool vlm_capture_triggered_{false};
   std::optional<geometry_msgs::msg::PoseStamped> recovery_start_pose_;

src/racing_control/test/test_racing_control_helpers.cpp

@@ -132,6 +132,37 @@ TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal)
   EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0));
 }
 
+TEST(RacingControlHelpers, AdvancesNextWaypointOnlyAfterItIsReached)
+{
+  const auto waypoints = racing_control::posesFromFlatDoubles(
+    {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0},
+    "map");
+
+  EXPECT_EQ(
+    racing_control::advanceReachedWaypointIndex(
+      waypoints, racing_control::poseFromXYYaw(0.65, 0.0, 0.0, "map"), 1, 0.30),
+    1U);
+  EXPECT_EQ(
+    racing_control::advanceReachedWaypointIndex(
+      waypoints, racing_control::poseFromXYYaw(1.05, 0.0, 0.0, "map"), 1, 0.30),
+    2U);
+}
+
+TEST(RacingControlHelpers, DynamicReplanningKeepsRemainingUnreachedWaypoints)
+{
+  const auto waypoints = racing_control::posesFromFlatDoubles(
+    {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0},
+    "map");
+
+  const auto remaining = racing_control::remainingWaypoints(waypoints, 1);
+
+  ASSERT_EQ(remaining.size(), 2U);
+  EXPECT_DOUBLE_EQ(remaining[0].pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(remaining[1].pose.position.x, 2.0);
+  EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size()).empty());
+  EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size() + 1).empty());
+}
+
 TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout)
 {
   EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2));