optimal

2026-07-30 09:24:47 +07:00
parent 4762a3032c
commit d8babff20b
50 changed files with 3280 additions and 119 deletions
--- a/test/navigation_server_test.cpp
+++ b/test/navigation_server_test.cpp
@@ -9,10 +9,13 @@
 *********************************************************************/
 #include <gtest/gtest.h>

+#include <atomic>
+#include <chrono>
 #include <cmath>
 #include <cstddef>
 #include <limits>
 #include <string>
+#include <thread>
 #include <vector>

 #include <robot_costmap_2d/layered_costmap.h>
@@ -236,8 +239,13 @@ TEST(NavigationServerTwist, StampComesFromTheControlLoopClockNotWallClock)
 {
  // Host loại lệnh quá hạn theo dấu này. Lấy giờ hệ thống lúc host hỏi sẽ làm một control loop đã
  // treo vẫn trông như đang phát lệnh tươi — đúng thứ dấu thời gian sinh ra để ngăn.
+  // Stamp chỉ tiến khi đang có yêu cầu (xem test StampFreezesWhenIdle...), nên phải có goal chạy.
  Fixture fixture;
  fixture.configure();
+  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
+  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();

  fixture.spin(1);
  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart, 1e-9);
@@ -247,6 +255,49 @@ TEST(NavigationServerTwist, StampComesFromTheControlLoopClockNotWallClock)
  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart + 12.0, 1e-9);
 }

+TEST(NavigationServerTwist, StampFreezesWhenIdleSoTeleopOwnsCmdVel)
+{
+  // Không có yêu cầu nào thì stamp phải ĐỨNG YÊN dù control loop vẫn chạy: host publish /cmd_vel
+  // qua cửa tươi 0.05 s (amr_publiser.cpp:361), stamp tươi mỗi cycle nghĩa là amr_node phát 0 ở
+  // 20 Hz vĩnh viễn và đè chết teleop/joystick (rqt_robot_steering 10 Hz — robot chỉ nhích rồi
+  // đứng im). Bản cũ chỉ đóng dấu trong executeCycle; đây là regression đã gặp trên sim.
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.spin(3);
+  EXPECT_TRUE(fixture.server_.getTwist().header.stamp.isZero())
+      << "chưa từng có yêu cầu mà stamp đã tươi — host sẽ phát 0 đè teleop";
+}
+
+TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
+{
+  // Lệnh 0 cuối cùng phải qua được cửa 0.05 s của host — kết thúc mà đóng băng stamp ngay thì
+  // robot giữ nguyên vận tốc chót. Cửa ân hạn 0.5 s; hết ân hạn stamp phải đứng yên trả /cmd_vel
+  // cho teleop.
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setNominalSpeed(0.3);  // [m/s]
+  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kGoalReached });
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+
+  fixture.spin(4);  // IDLE -> PLANNING -> CONTROLLING -> tới đích (terminal)
+  ASSERT_FALSE(fixture.server_.loop().hasActiveRequest());
+
+  // Ngay sau khi kết thúc: còn trong ân hạn, stamp vẫn tiến để host phát lệnh dừng.
+  const double stamp_in_grace = fixture.server_.getTwist().header.stamp.toSec();
+  fixture.spin(1);
+  EXPECT_GT(fixture.server_.getTwist().header.stamp.toSec(), stamp_in_grace)
+      << "stamp đóng băng ngay khi kết thúc — lệnh dừng cuối không bao giờ được publish";
+
+  // Chạy qua hết cửa ân hạn (0.5 s = 10 cycle) rồi thêm vài cycle: stamp phải đứng yên.
+  fixture.spin(12);
+  const double stamp_frozen = fixture.server_.getTwist().header.stamp.toSec();
+  fixture.spin(3);
+  EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_frozen, 1e-9)
+      << "hết ân hạn mà stamp vẫn tươi — teleop không bao giờ lấy lại được /cmd_vel";
+}
+
 TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
 {
  Fixture fixture;
@@ -552,6 +603,257 @@ TEST(NavigationServerLimits, OdometryReachesTheControllerAsMeasuredVelocity)
  EXPECT_NEAR(fixture.controller_.measuredVelocity().angular.z, -0.17, 1e-9);
 }

+// ================================================================================================
+// pause / resume / cancel — host gọi từ thread khác
+//
+// OPC-UA và VDA5050 chạy thread riêng (amr_control.cpp:159, 184) và gọi thẳng ba hàm này.
+// `ControlLoop` tự khai là không thread-safe, và `requestPause()` còn ghi HAI cờ không nguyên tử —
+// xen kẽ với `requestResume()` có thể để lại cả hai cùng false. Nên chúng chỉ được ghi nhận ở đây,
+// rồi chuyển xuống lõi trên control thread.
+// ================================================================================================
+
+TEST(NavigationServerLifecycle, PauseTakesEffectOnTheNextCycleNotImmediately)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+
+  fixture.server_.pause();
+  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling)
+      << "pause() đi thẳng vào lõi từ thread host";
+
+  fixture.spin(1);
+  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
+}
+
+TEST(NavigationServerLifecycle, ResumeAfterPauseReturnsToControlling)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+
+  fixture.server_.pause();
+  fixture.spin(1);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
+
+  fixture.server_.resume();
+  fixture.spin(1);
+  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+}
+
+TEST(NavigationServerLifecycle, PauseThenResumeWithinOneCycleEndsResumed)
+{
+  // Hai cờ đối nghịch được đặt dưới cùng một lock, nên lệnh sau luôn thắng lệnh trước — không có
+  // trạng thái "cả hai cùng false" như bản ghi hai cờ rời rạc.
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+
+  fixture.server_.pause();
+  fixture.server_.resume();
+  fixture.spin(1);
+
+  EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+}
+
+TEST(NavigationServerLifecycle, CancelWinsOverAPauseRequestedInTheSameCycle)
+{
+  // "Tạm dừng rồi huỷ" và "huỷ rồi tạm dừng" phải cho cùng kết quả: huỷ thắng.
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
+
+  fixture.server_.pause();
+  fixture.server_.cancel();
+  fixture.spin(1);
+
+  EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause thắng cancel";
+}
+
+TEST(NavigationServerLifecycle, LifecycleRequestIsConsumedExactlyOnce)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  fixture.spin(2);
+
+  fixture.server_.pause();
+  fixture.spin(1);
+  ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
+
+  // Không có lệnh mới: cờ đã bị tiêu thụ, các cycle sau không được tự tạm dừng lại lần nữa.
+  fixture.server_.resume();
+  fixture.spin(3);
+  EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused);
+}
+
+// ================================================================================================
+// Control thread
+//
+// Contract `BaseNavigation` KHÔNG có hàm spin nào: host nạp plugin, gọi initialize(), rồi chỉ tương
+// tác qua moveTo/getTwist/getFeedback. Runtime vì thế phải TỰ LÁI mình, đúng như bản cũ (thread
+// planner + action server). Thiếu control thread thì goal nằm im trong chỗ chờ vĩnh viễn — không
+// cycle nào chạy, không state nào đổi, và không log gì cả. Đó là lỗi đã thật sự xảy ra trên sim.
+// ================================================================================================
+
+TEST(NavigationServerControlThread, RunsCyclesWithoutAnyoneCallingSpinOnce)
+{
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+
+  // Ở đây KHÔNG gọi spin() của fixture: chính control thread phải đẩy state đi.
+  ASSERT_TRUE(fixture.server_.startControlThread(200.0));  // [Hz]
+
+  bool left_idle = false;
+  for (int i = 0; i < 500 && !left_idle; ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
+    left_idle = fixture.server_.loop().state() != NavigationState::kIdle;
+  }
+  fixture.server_.stopControlThread();
+
+  EXPECT_TRUE(left_idle) << "goal được nhận nhưng không cycle nào chạy — thiếu control thread";
+}
+
+TEST(NavigationServerControlThread, RefusesToStartBeforeTheLoopIsConfigured)
+{
+  NavigationServer server;
+  EXPECT_FALSE(server.startControlThread(20.0));
+  EXPECT_FALSE(server.controlThreadRunning());
+}
+
+TEST(NavigationServerControlThread, RefusesNonPositiveFrequency)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  EXPECT_FALSE(fixture.server_.startControlThread(0.0));
+  EXPECT_FALSE(fixture.server_.startControlThread(-5.0));
+}
+
+TEST(NavigationServerControlThread, SecondStartIsRefusedAndStopIsIdempotent)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  ASSERT_TRUE(fixture.server_.startControlThread(100.0));
+  EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "khởi động thread thứ hai";
+
+  fixture.server_.stopControlThread();
+  fixture.server_.stopControlThread();  // không được treo hay sập
+  EXPECT_FALSE(fixture.server_.controlThreadRunning());
+}
+
+TEST(NavigationServerControlThread, DestructorStopsTheThread)
+{
+  // Thread chạm loop_, runtime_ và sensors_ mỗi cycle; huỷ chúng khi thread còn sống là hỏng ở chỗ
+  // không truy được.
+  Fixture fixture;
+  fixture.configure();
+  ASSERT_TRUE(fixture.server_.startControlThread(100.0));
+  SUCCEED();  // destructor của fixture phải join, không treo
+}
+
+// ================================================================================================
+// Dữ liệu hiển thị — host gọi từ BỐN ros::Timer khác nhau
+// ================================================================================================
+
+TEST(NavigationServerPlannerData, GettersDoNotShareMutableState)
+{
+  // `getGlobalData` và `getLocalData` mỗi cái được gọi từ HAI timer (costmap và plan). Ghi vào
+  // `global_data_`/`local_data_` dùng chung đã gây `std::bad_alloc` rồi hỏng heap và giết tiến trình.
+  Fixture fixture;
+  fixture.configure();
+  fixture.spin(1);
+
+  robot::move_base_core::PlannerDataOutput a = fixture.server_.getGlobalData();
+  robot::move_base_core::PlannerDataOutput b = fixture.server_.getGlobalData();
+
+  a.plan.poses.clear();
+  EXPECT_TRUE(b.plan.poses.empty() || !a.plan.poses.empty())
+      << "hai lần gọi trả về cùng một vùng nhớ";
+  EXPECT_NO_THROW({ (void)fixture.server_.getLocalData(); });
+}
+
+TEST(NavigationServerPlannerData, ConcurrentGettersDoNotCorruptEachOther)
+{
+  // Không chứng minh được không có race (cần ThreadSanitizer), nhưng chạy đúng hình dạng lời gọi
+  // của host: bốn thread cùng đọc trong khi control thread cùng ghi bộ đệm.
+  Fixture fixture;
+  fixture.configure();
+  fixture.controller_.setScript({ ControllerScript::kOk });
+  ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10))
+      << fixture.server_.lastRejectReason();
+  ASSERT_TRUE(fixture.server_.startControlThread(200.0));
+
+  std::atomic<bool> stop{ false };
+  std::vector<std::thread> readers;
+  for (int i = 0; i < 4; ++i)
+  {
+    readers.emplace_back([&fixture, &stop, i]() {
+      while (!stop.load())
+      {
+        if (i % 2 == 0)
+        {
+          (void)fixture.server_.getGlobalData();
+        }
+        else
+        {
+          (void)fixture.server_.getLocalData();
+        }
+      }
+    });
+  }
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  stop.store(true);
+  for (auto& t : readers)
+  {
+    t.join();
+  }
+  fixture.server_.stopControlThread();
+
+  SUCCEED();
+}
+
+TEST(NavigationServerPlannerData, PlanIsStampedWithTheControlLoopClock)
+{
+  Fixture fixture;
+  fixture.configure();
+
+  fixture.clock_.setTime(kClockStart + 7.0);
+  fixture.spin(1);
+
+  EXPECT_NEAR(fixture.server_.getGlobalData().plan.header.stamp.toSec(), kClockStart + 7.0, 1e-9)
+      << "plan mang dấu thời gian khác đồng hồ control loop — host sẽ coi là quá hạn và bỏ qua";
+}
+
 int main(int argc, char** argv)
 {
  ::testing::InitGoogleTest(&argc, argv);