first commit

2026-07-29 15:45:16 +07:00
commit 4762a3032c
56 changed files with 15310 additions and 0 deletions
--- a/test/controller_runner_test.cpp
+++ b/test/controller_runner_test.cpp
@@ -0,0 +1,381 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — test ControllerRunner: nạp plugin thật qua Boost.DLL, trần vận tốc phải tới được
+ * plugin, và mọi đường lỗi phải trả về "không có lệnh" chứ không để dữ liệu hỏng đi tiếp.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <cstdint>
+#include <cstdlib>
+#include <limits>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/runners/controller_runner.h>
+
+namespace
+{
+using move_base2::ControllerRunner;
+
+/// [m/s] Lệnh nền của plugin test khi chưa đặt trần và vận tốc đo được bằng 0.
+constexpr double kBaseSpeed = 0.25;
+/// [rad/s]
+constexpr double kBaseYawRate = 0.40;
+
+/**
+ * @brief Con trỏ costmap giả.
+ *
+ * `Costmap2DROBOT` không dựng được trong unit test (cần `tf3::BufferCore` thật và cây config đầy
+ * đủ). An toàn ở đây vì `test_local_planner.cpp` không alias nào chạm vào con trỏ này — nó chỉ đi
+ * qua `initialize()` rồi bị bỏ. Đường có costmap thật thuộc test tích hợp (Phase 5).
+ */
+robot_costmap_2d::Costmap2DROBOT* dummyCostmap()
+{
+  static std::uintptr_t placeholder = 0;
+  return reinterpret_cast<robot_costmap_2d::Costmap2DROBOT*>(&placeholder);
+}
+
+std::vector<robot_geometry_msgs::PoseStamped> makePlan(std::size_t poses = 3)
+{
+  std::vector<robot_geometry_msgs::PoseStamped> plan;
+  for (std::size_t i = 0; i < poses; ++i)
+  {
+    robot_geometry_msgs::PoseStamped pose;
+    pose.header.frame_id = "map";
+    pose.pose.position.x = static_cast<double>(i);  // [m]
+    pose.pose.orientation.w = 1.0;
+    plan.push_back(pose);
+  }
+  return plan;
+}
+
+robot_geometry_msgs::Vector3 vec(double x, double y = 0.0, double z = 0.0)
+{
+  robot_geometry_msgs::Vector3 v;
+  v.x = x;
+  v.y = y;
+  v.z = z;
+  return v;
+}
+
+robot_geometry_msgs::Twist twist(double vx, double wz = 0.0)
+{
+  robot_geometry_msgs::Twist t;
+  t.linear.x = vx;   // [m/s]
+  t.angular.z = wz;  // [rad/s]
+  return t;
+}
+
+class Fixture
+{
+public:
+  explicit Fixture(const std::string& name = "TestControllerOk")
+  {
+    robot::NodeHandle nh;
+    std::string error;
+    ok_ = runner_.configure(nh, nullptr, dummyCostmap(), name, error);
+    error_ = error;
+  }
+
+  bool ok() const
+  {
+    return ok_;
+  }
+
+  const std::string& error() const
+  {
+    return error_;
+  }
+
+  ControllerRunner runner_;
+
+private:
+  bool ok_ = false;
+  std::string error_;
+};
+
+}  // namespace
+
+// ================================================================================================
+// Cấu hình và nạp plugin
+// ================================================================================================
+
+TEST(ControllerRunner, RefusesNullCostmap)
+{
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+
+  EXPECT_FALSE(runner.configure(nh, nullptr, nullptr, "TestControllerOk", error));
+  EXPECT_FALSE(error.empty());
+  EXPECT_FALSE(runner.configured());
+}
+
+TEST(ControllerRunner, ConfigureFailsWhenTheInitialControllerCannotBeLoaded)
+{
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+
+  EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), "TestControllerMissing", error));
+  EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
+}
+
+TEST(ControllerRunner, LoadsTheInitialControllerAndReportsItAsActive)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
+  EXPECT_EQ(fixture.runner_.loadedCount(), 1U);
+}
+
+TEST(ControllerRunner, SwapsBetweenControllersAndReusesLoadedLibraries)
+{
+  // swapPlanner chạy ở cửa vào mỗi yêu cầu (profile position/docking/...). Đổi qua lại không được
+  // dlopen lại.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerSecondary"));
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U);
+
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerOk"));
+  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại controller cũ mà vẫn nạp lại thư viện";
+}
+
+TEST(ControllerRunner, FailedSwapKeepsThePreviousControllerActive)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.swapPlanner("TestControllerMissing"));
+  EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
+}
+
+TEST(ControllerRunner, SwapBeforeConfigureIsRefused)
+{
+  ControllerRunner runner;
+  EXPECT_FALSE(runner.swapPlanner("TestControllerOk"));
+}
+
+// ================================================================================================
+// Trần vận tốc — đường tầng an toàn hạ tốc độ robot (bước 12)
+// ================================================================================================
+
+TEST(ControllerRunner, ForwardVelocityLimitReachesThePlugin)
+{
+  // Nếu lời gọi này không tới được plugin thì tầng an toàn yêu cầu giảm tốc mà robot vẫn chạy
+  // nguyên tốc độ planner — và không có dấu hiệu nào cả.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  ASSERT_NEAR(cmd.linear.x, kBaseSpeed, 1e-9);
+
+  ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.10)));  // [m/s]
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "trần vận tốc không tới được plugin";
+}
+
+TEST(ControllerRunner, AngularVelocityLimitReachesThePlugin)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  ASSERT_NEAR(cmd.angular.z, kBaseYawRate, 1e-9);
+
+  ASSERT_TRUE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.15)));  // [rad/s]
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.angular.z, 0.15, 1e-9);
+}
+
+TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded)
+{
+  // Thứ tự khởi tạo không do move_base2 quyết: host có thể đặt trần trước khi controller được nạp.
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+
+  EXPECT_TRUE(runner.setTwistLinear(vec(0.08)));  // [m/s], chưa có controller nào
+  ASSERT_TRUE(runner.swapPlanner("TestControllerOk"));
+  ASSERT_TRUE(runner.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(runner.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "trần đặt trước khi nạp controller bị mất";
+}
+
+TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
+{
+  // Trần thuộc về YÊU CẦU chứ không thuộc về instance planner. Instance mới không biết gì về trần
+  // đã đặt — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an toàn cho phép.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.07)));  // [m/s]
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerSecondary"));
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "đổi controller làm mất trần vận tốc đang có hiệu lực";
+}
+
+TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
+{
+  // Host phải biết trần của nó không có hiệu lực, thay vì tưởng đã đặt được.
+  Fixture fixture("TestControllerRefusesLimits");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.setTwistLinear(vec(0.10)));
+  EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.10)));
+}
+
+TEST(ControllerRunner, NonFiniteLimitIsRejected)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  const double nan = std::numeric_limits<double>::quiet_NaN();
+  EXPECT_FALSE(fixture.runner_.setTwistLinear(vec(nan)));
+  EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, nan)));
+}
+
+// ================================================================================================
+// Vận tốc đo được
+// ================================================================================================
+
+TEST(ControllerRunner, MeasuredVelocityReachesThePlugin)
+{
+  // Interface gen-1 nhận vận tốc hiện tại làm tham số của computeVelocityCommands. Bản cũ đưa nó
+  // vào bằng con trỏ tới bộ nhớ host ghi (`setOdom(&odometry_)`) — một data race không có gì bảo
+  // vệ. Ở đây truyền theo giá trị.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  fixture.runner_.setMeasuredVelocity(twist(0.30));  // [m/s]
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "vận tốc đo được không tới được plugin";
+}
+
+TEST(ControllerRunner, NonFiniteMeasuredVelocityIsDroppedAndTheOldValueKept)
+{
+  // Nhiều local planner dùng vận tốc hiện tại làm mốc giới hạn gia tốc; NaN ở đó lan ra toàn bộ
+  // cost function.
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  fixture.runner_.setMeasuredVelocity(twist(0.20));
+  fixture.runner_.setMeasuredVelocity(twist(std::numeric_limits<double>::quiet_NaN()));
+
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.20, 1e-9);
+}
+
+// ================================================================================================
+// Đường lỗi
+// ================================================================================================
+
+TEST(ControllerRunner, EmptyPlanIsRefused)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.setPlan({}));
+}
+
+TEST(ControllerRunner, SetPlanWithoutAControllerFails)
+{
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+
+  EXPECT_FALSE(runner.setPlan(makePlan()));
+}
+
+TEST(ControllerRunner, ComputeWithoutAControllerYieldsNoCommand)
+{
+  robot::NodeHandle nh;
+  ControllerRunner runner;
+  std::string error;
+  ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error;
+
+  robot_geometry_msgs::Twist cmd;
+  EXPECT_FALSE(runner.computeVelocityCommands(cmd));
+}
+
+TEST(ControllerRunner, PluginReturningNoCommandIsPassedThroughAsFalse)
+{
+  Fixture fixture("TestControllerNoCommand");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
+}
+
+TEST(ControllerRunner, NaNCommandIsBlockedAtTheBoundary)
+{
+  // VelocityArbiter cũng chặn NaN, nhưng chặn tại nguồn cho biết ĐÚNG plugin nào đang trả dữ liệu
+  // hỏng — arbiter chỉ thấy một con số vô nghĩa không rõ từ đâu.
+  Fixture fixture("TestControllerNaN");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "lệnh chứa NaN vẫn được ghi ra ngoài";
+}
+
+TEST(ControllerRunner, ExceptionFromThePluginIsContained)
+{
+  Fixture fixture("TestControllerThrowing");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist cmd;
+  EXPECT_NO_THROW({ EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd)); });
+}
+
+TEST(ControllerRunner, CommandIsClearedBeforeEveryAttempt)
+{
+  // Bên gọi dùng lại cùng một biến qua nhiều cycle. Trả false mà để nguyên lệnh cũ trong đó là mời
+  // tầng trên phát lại một lệnh đã hết hạn.
+  Fixture fixture("TestControllerNoCommand");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  robot_geometry_msgs::Twist cmd = twist(9.0, 9.0);
+  EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.0, 1e-9);
+  EXPECT_NEAR(cmd.angular.z, 0.0, 1e-9);
+}
+
+int main(int argc, char** argv)
+{
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
+
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}