optimal

2026-07-30 09:24:47 +07:00
parent 4762a3032c
commit d8babff20b
50 changed files with 3280 additions and 119 deletions
--- a/test/walking_skeleton_test.cpp
+++ b/test/walking_skeleton_test.cpp
@@ -1002,6 +1002,115 @@ TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt)
      << join(fixture.states());
 }

+// ================================================================================================
+// Preempt — goal mới thay goal cũ NGAY
+//
+// Bấm goal mới nghĩa là goal cũ không còn muốn nữa. Xếp hàng chờ robot đi hết chặng cũ là hành vi
+// không ai mong đợi, và mission layer cũng đã chốt "preempt ngay" (Q1, Phase 2).
+// ================================================================================================
+
+TEST(ControlLoopPreempt, NewGoalWhileControllingReplansImmediately)
+{
+  Fixture fixture;
+  fixture.controller_.setScript({ControllerScript::kOk});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
+  fixture.stepOnce();  // IDLE -> PLANNING
+  fixture.stepOnce();  // -> CONTROLLING
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  const std::size_t plans_before = fixture.planner_.makePlanCount();
+
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
+  fixture.stepOnce();
+
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
+      << "goal mới nằm chờ thay vì thay goal cũ ngay";
+  EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "không lập plan lại cho goal mới";
+}
+
+TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
+{
+  Fixture fixture;
+  fixture.planner_.setLatencyCycles(50);  // plan cũ còn lâu mới xong
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
+  fixture.stepOnce();
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
+
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
+  fixture.stepOnce();
+
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
+  EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "lượt lập plan của goal cũ không bị huỷ";
+}
+
+TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
+{
+  // Bất biến quan trọng nhất: báo kết quả ĐÚNG MỘT LẦN và ĐÚNG ID. Preempt báo kết quả chặng cũ và
+  // nhận chặng mới trong cùng một cycle — dùng nhầm id thì mission layer mất dấu cả hai chặng.
+  Fixture fixture;
+  fixture.controller_.setScript({ControllerScript::kOk});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 11), reason)) << reason;
+  fixture.stepOnce();
+  fixture.stepOnce();
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
+  fixture.stepOnce();
+
+  EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "chặng bị thay không được báo đúng một lần";
+  EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "chặng MỚI bị báo kết quả ngay khi nhận";
+}
+
+TEST(ControlLoopPreempt, PreemptedGoalStillFinishesTheNewOne)
+{
+  // Preempt không được để lại trạng thái nửa vời: chặng mới phải chạy tới cùng như bình thường.
+  Fixture fixture;
+  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kOk,
+                                 ControllerScript::kGoalReached});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 11), reason)) << reason;
+  fixture.stepOnce();
+  fixture.stepOnce();
+
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
+  fixture.run();
+
+  EXPECT_FALSE(fixture.hitLimit()) << join(fixture.states());
+  EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED");
+  EXPECT_EQ(fixture.mission_.reportCountFor(22), 1u);
+}
+
+TEST(ControlLoopPreempt, NewGoalDuringRecoveryCancelsTheRunningBehavior)
+{
+  ControlLoopConfig config = baseConfig();
+  config.state_machine.planner_patience = 0.05;  // [s] vào recovery nhanh
+  Fixture fixture(config);
+  fixture.planner_.setScript({PlannerScript::kFail});
+  fixture.recovery_.setScript({RecoveryScript::kRunning});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
+  for (int i = 0; i < 6 && fixture.loop_.state() != NavigationState::kRecovering; ++i)
+  {
+    fixture.stepOnce();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kRecovering) << join(fixture.states());
+
+  fixture.planner_.setScript({PlannerScript::kOk});
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason;
+  fixture.stepOnce();
+
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
+  EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "behavior đang chạy không được bảo dừng";
+}
+
 int main(int argc, char** argv)
 {
  testing::InitGoogleTest(&argc, argv);