move_base2/test/action_runner_test.cpp

/*********************************************************************
 *
 * 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 <action_core/action_handler.h>
#include <move_base2/runners/action_runner.h>

#include "fake_ports.h"

namespace
{
using action_core::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, const action_core::ActionContext& /*ctx*/,
                 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;
  }

  action_core::ActionTick update(const robot::Time& /*now*/) override
  {
    ++update_count;
    action_core::ActionTick tick;
    tick.status = next_status;
    return tick;
  }

  void cancel() override
  {
    ++cancel_count;
  }

  bool configure_ok = true;
  bool start_ok = true;
  action_core::ActionStatus next_status = action_core::ActionStatus::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 = action_core::ActionStatus::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)) << "missing ClockPort means handlers have no timeout "
                                        "reference";
}

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();
}