first commit

2026-07-29 15:45:16 +07:00
commit 4762a3032c
56 changed files with 15310 additions and 0 deletions
--- a/test/action_runner_test.cpp
+++ b/test/action_runner_test.cpp
@@ -0,0 +1,363 @@
+/*********************************************************************
+ *
+ * move_base2 — kiểm ActionRunner: định tuyến theo actionType, vòng đời tick, và timeout tầng 1.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <move_base2/runners/action_runner.h>
+
+#include "fake_ports.h"
+
+namespace
+{
+using move_base2::ActionHandler;
+using move_base2::ActionRunner;
+using move_base2::ActionTick;
+using move_base2::testing::FakeClockPort;
+
+robot_protocol_msgs::Action makeAction(const std::string& type, const std::string& id = "a1")
+{
+  robot_protocol_msgs::Action action;
+  action.actionType = type;
+  action.actionId = id;
+  return action;
+}
+
+/**
+ * @brief Handler viết hoàn toàn trong test.
+ *
+ * Sự tồn tại của nó là bài kiểm tra thật cho tính plugin: thêm một loại action mới mà không sửa
+ * file nào trong `src/` của gói.
+ */
+class ScriptedHandler final : public ActionHandler
+{
+public:
+  explicit ScriptedHandler(std::vector<std::string> types) : types_(std::move(types))
+  {
+  }
+
+  bool configure(const std::string& name, robot::NodeHandle& /*nh*/) override
+  {
+    name_ = name;
+    return configure_ok;
+  }
+
+  std::vector<std::string> supportedActionTypes() const override
+  {
+    return types_;
+  }
+
+  bool start(const robot_protocol_msgs::Action& action, const robot::Time& /*now*/) override
+  {
+    ++start_count;
+    last_action_id = action.actionId;
+    return start_ok;
+  }
+
+  ActionTick update(const robot::Time& /*now*/) override
+  {
+    ++update_count;
+    ActionTick tick;
+    tick.status = next_status;
+    return tick;
+  }
+
+  void cancel() override
+  {
+    ++cancel_count;
+  }
+
+  bool configure_ok = true;
+  bool start_ok = true;
+  ActionTick::Status next_status = ActionTick::Status::kRunning;
+
+  int start_count = 0;
+  int update_count = 0;
+  int cancel_count = 0;
+  std::string last_action_id;
+
+private:
+  std::vector<std::string> types_;
+  std::string name_;
+};
+
+struct Rig
+{
+  Rig()
+  {
+    runner.setClock(&clock);
+  }
+
+  bool load(const std::string& ns)
+  {
+    runner.setNamespace(ns);
+    robot::NodeHandle nh;
+    return runner.configure(nh);
+  }
+
+  FakeClockPort clock{1000.0};
+  ActionRunner runner;
+};
+
+// ------------------------------------------------------------------------------------------------
+// Đăng ký thủ công — kiểm định tuyến và vòng đời mà không cần .so
+// ------------------------------------------------------------------------------------------------
+
+TEST(ActionRunner, RoutesByActionType)
+{
+  Rig rig;
+  auto pick = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"});
+  auto drop = std::make_shared<ScriptedHandler>(std::vector<std::string>{"drop"});
+  ASSERT_TRUE(rig.runner.registerHandler(pick));
+  ASSERT_TRUE(rig.runner.registerHandler(drop));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  ASSERT_TRUE(rig.runner.start(makeAction("drop", "d7")));
+
+  EXPECT_EQ(drop->start_count, 1);
+  EXPECT_EQ(pick->start_count, 0);
+  EXPECT_EQ(drop->last_action_id, "d7");
+}
+
+TEST(ActionRunner, OneHandlerCanTakeSeveralTypes)
+{
+  Rig rig;
+  auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick", "drop"});
+  ASSERT_TRUE(rig.runner.registerHandler(handler));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  EXPECT_TRUE(rig.runner.start(makeAction("pick")));
+  EXPECT_TRUE(rig.runner.start(makeAction("drop")));
+  EXPECT_EQ(handler->start_count, 2);
+}
+
+TEST(ActionRunner, RejectsDuplicateActionType)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+
+  // Hai handler cùng nhận một type thì định tuyến phụ thuộc thứ tự nạp — phải từ chối, không ghi đè.
+  EXPECT_FALSE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+}
+
+TEST(ActionRunner, RejectsHandlerWithoutTypes)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.runner.registerHandler(std::make_shared<ScriptedHandler>(
+      std::vector<std::string>{})));
+  EXPECT_FALSE(rig.runner.registerHandler(nullptr));
+}
+
+TEST(ActionRunner, UnknownActionTypeFailsStart)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  EXPECT_FALSE(rig.runner.start(makeAction("charge")));
+}
+
+TEST(ActionRunner, EmptyActionTypeFailsStart)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  EXPECT_FALSE(rig.runner.start(makeAction("")));
+}
+
+TEST(ActionRunner, StartBeforeConfigureFails)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.runner.registerHandler(
+      std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"})));
+
+  EXPECT_FALSE(rig.runner.start(makeAction("pick")));
+}
+
+TEST(ActionRunner, HandlerRefusingStartIsReported)
+{
+  Rig rig;
+  auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"});
+  handler->start_ok = false;
+  ASSERT_TRUE(rig.runner.registerHandler(handler));
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  EXPECT_FALSE(rig.runner.start(makeAction("pick")));
+  // Không được tick tiếp sau khi start hỏng.
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed);
+  EXPECT_EQ(handler->update_count, 0);
+}
+
+TEST(ActionRunner, UpdateWithoutActiveActionFailsInsteadOfCrashing)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions_empty"));
+
+  const ActionTick tick = rig.runner.update();
+
+  EXPECT_EQ(tick.status, ActionTick::Status::kFailed);
+  EXPECT_FALSE(tick.message.empty());
+}
+
+TEST(ActionRunner, TicksUntilHandlerFinishes)
+{
+  Rig rig;
+  auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"});
+  ASSERT_TRUE(rig.runner.registerHandler(handler));
+  ASSERT_TRUE(rig.load("actions_empty"));
+  ASSERT_TRUE(rig.runner.start(makeAction("pick")));
+
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
+
+  handler->next_status = ActionTick::Status::kSucceeded;
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kSucceeded);
+
+  // Sau khi kết thúc, action không còn active: tick thêm là lỗi thứ tự gọi, không phải kRunning.
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed);
+  EXPECT_EQ(handler->update_count, 3);
+}
+
+TEST(ActionRunner, CancelReachesHandlerAndClearsActive)
+{
+  Rig rig;
+  auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"pick"});
+  ASSERT_TRUE(rig.runner.registerHandler(handler));
+  ASSERT_TRUE(rig.load("actions_empty"));
+  ASSERT_TRUE(rig.runner.start(makeAction("pick")));
+
+  rig.runner.cancel();
+
+  EXPECT_EQ(handler->cancel_count, 1);
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed);
+}
+
+TEST(ActionRunner, CancelWithoutActiveActionIsSafe)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions_empty"));
+  rig.runner.cancel();
+  SUCCEED();
+}
+
+TEST(ActionRunner, ConfigureRequiresClock)
+{
+  ActionRunner runner;  // không setClock
+  robot::NodeHandle nh;
+  EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort thì handler không có mốc timeout";
+}
+
+TEST(ActionRunner, EmptyHandlerListIsValid)
+{
+  // Hệ không có thiết bị nào: mọi mission đều nav-only, và ControlLoop::submit đã từ chối yêu cầu
+  // mang action ngay tại cửa.
+  Rig rig;
+  EXPECT_TRUE(rig.load("actions_empty"));
+  EXPECT_EQ(rig.runner.handlerCount(), 0u);
+  EXPECT_FALSE(rig.runner.start(makeAction("pick")));
+}
+
+// ------------------------------------------------------------------------------------------------
+// Nạp thật qua Boost.DLL — đúng đường runtime đi
+// ------------------------------------------------------------------------------------------------
+
+TEST(ActionRunner, LoadsHandlerPluginFromConfig)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions"));
+
+  ASSERT_EQ(rig.runner.handlerCount(), 1u);
+  EXPECT_NE(rig.runner.find("wait"), nullptr);
+  EXPECT_NE(rig.runner.find("pick"), nullptr);
+  EXPECT_EQ(rig.runner.find("charge"), nullptr);
+}
+
+TEST(ActionRunner, LoadedHandlerRunsForConfiguredDuration)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions"));
+  ASSERT_TRUE(rig.runner.start(makeAction("wait", "w1")));  // duration: 2.0 s
+
+  rig.clock.advance(1.0);
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
+
+  rig.clock.advance(1.0);
+  EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kSucceeded);
+}
+
+TEST(ActionRunner, HandlerTimesOutOnItsOwnWithoutStateMachineHelp)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions_slow"));  // hang: true, timeout 3 s
+  ASSERT_TRUE(rig.runner.start(makeAction("wait")));
+
+  rig.clock.advance(2.0);
+  ASSERT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
+
+  rig.clock.advance(1.5);
+  const ActionTick tick = rig.runner.update();
+
+  // Timeout TẦNG 1: handler tự chịu trách nhiệm, không dựa vào action_patience (mặc định tắt).
+  EXPECT_EQ(tick.status, ActionTick::Status::kFailed);
+  EXPECT_NE(tick.message.find("timeout"), std::string::npos);
+}
+
+TEST(ActionRunner, HangingHandlerWithoutTimeoutIsRejectedAtConfigure)
+{
+  // Treo + tắt timeout = action chạy vĩnh viễn. Contract cấm, nên phải chặn lúc khởi động.
+  Rig rig;
+  EXPECT_FALSE(rig.load("actions_hang_forever"));
+  EXPECT_EQ(rig.runner.handlerCount(), 0u);
+}
+
+TEST(ActionRunner, MissingLibraryPathFailsConfigure)
+{
+  Rig rig;
+  EXPECT_FALSE(rig.load("actions_missing_library"));
+  EXPECT_EQ(rig.runner.handlerCount(), 0u);
+}
+
+TEST(ActionRunner, HandlerWithTimeoutBelowDurationIsRejectedAtConfigure)
+{
+  // Cấu hình khiến action LUÔN hỏng vì timeout — phải chặn lúc khởi động, không phải lúc chạy.
+  Rig rig;
+  EXPECT_FALSE(rig.load("actions_bad"));
+  EXPECT_EQ(rig.runner.handlerCount(), 0u);
+}
+
+TEST(ActionRunner, ConfigureTwiceRejected)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("actions"));
+
+  robot::NodeHandle nh;
+  EXPECT_FALSE(rig.runner.configure(nh));
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+#ifdef MOVE_BASE2_TEST_CONFIG_DIR
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+#endif
+#ifdef MOVE_BASE2_TEST_LIBRARY_DIR
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
+#endif
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}