recovery_core/test/registry_test.cpp

/*********************************************************************
 *
 * Kiểm đường nạp plugin thật: YAML -> library_path -> Boost.DLL -> configure.
 *
 * Bản test cũ của gói KHÔNG kiểm được gì: nó `return` khi không nạp được plugin nào và `main` trả 0,
 * nên "không plugin nào chạy" cũng in [PASS]. Test này phải fail được.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cstdlib>
#include <memory>
#include <string>

#include <robot/node_handle.h>

#include <recovery_core/recovery_registry.h>

#include "recovery_test_utils.h"

namespace
{
using recovery_core::RecoveryOutputType;
using recovery_test::MockBehavior;
using recovery_test::VelocityRig;

TEST(Registry, LoadsDeclaredBehaviorsInOrder)
{
  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;

  ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));

  // Thứ tự CHÍNH LÀ hành vi: caller thử behavior 0 trước.
  ASSERT_EQ(registry.size(), 3u);
  EXPECT_EQ(registry.nameAt(0), "wait");
  EXPECT_EQ(registry.nameAt(1), "rotate");
  EXPECT_EQ(registry.nameAt(2), "back_up");
}

TEST(Registry, LoadedBehaviorsReportCorrectOutputKind)
{
  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;

  ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
  ASSERT_EQ(registry.size(), 3u);

  EXPECT_EQ(registry.at(0)->outputKind(), RecoveryOutputType::kNone);      // wait
  EXPECT_EQ(registry.at(1)->outputKind(), RecoveryOutputType::kVelocity);  // rotate
  EXPECT_EQ(registry.at(2)->outputKind(), RecoveryOutputType::kVelocity);  // back_up
}

TEST(Registry, PerInstanceParamsComeFromItsOwnNamespace)
{
  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;

  ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));

  // `recovery/rotate/timeout: 20.0`, `recovery/back_up/timeout: 15.0`, `recovery/wait` không khai.
  EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "rotate")->timeout(), 20.0);
  EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "back_up")->timeout(), 15.0);
  EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "wait")->timeout(), 0.0);
}

TEST(Registry, MissingLibraryPathIsReportedAndFails)
{
  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;

  // `GhostRecovery` được khai trong danh sách nhưng không có khoá library_path.
  EXPECT_FALSE(registry.loadFromConfig(nh, "recovery_missing_library", rig.ctx));
  EXPECT_EQ(registry.size(), 0u);
}

TEST(Registry, OneBadBehaviorDoesNotDropTheGoodOnes)
{
  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;

  // Một đường phục hồi hỏng không nên xoá sạch các đường còn lại.
  EXPECT_FALSE(registry.loadFromConfig(nh, "recovery_partial", rig.ctx));
  ASSERT_EQ(registry.size(), 1u);
  EXPECT_EQ(registry.nameAt(0), "wait");
}

TEST(Registry, MissingBehaviorListFails)
{
  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;

  EXPECT_FALSE(registry.loadFromConfig(nh, "namespace_khong_ton_tai", rig.ctx));
  EXPECT_EQ(registry.size(), 0u);
}

TEST(Registry, IndexOutOfRangeIsSafe)
{
  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;

  ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));

  EXPECT_EQ(registry.at(99), nullptr);
  EXPECT_TRUE(registry.nameAt(99).empty());
}

TEST(Registry, RegisterBehaviorRejectsNull)
{
  recovery_core::RecoveryRegistry registry;

  EXPECT_FALSE(registry.registerBehavior(nullptr));
  EXPECT_EQ(registry.size(), 0u);
}

TEST(Registry, RegisterBehaviorAppendsInOrder)
{
  recovery_core::RecoveryRegistry registry;

  auto first = std::make_shared<MockBehavior>(RecoveryOutputType::kNone);
  auto second = std::make_shared<MockBehavior>(RecoveryOutputType::kNone);

  ASSERT_TRUE(registry.registerBehavior(first));
  ASSERT_TRUE(registry.registerBehavior(second));

  ASSERT_EQ(registry.size(), 2u);
  EXPECT_EQ(registry.at(0), first.get());
  EXPECT_EQ(registry.at(1), second.get());
}

TEST(Registry, ClearReleasesBehaviors)
{
  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;

  ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
  ASSERT_GT(registry.size(), 0u);

  registry.clear();
  EXPECT_EQ(registry.size(), 0u);
}

TEST(Registry, BehaviorsStayUsableAfterLoaderScopeEnds)
{
  VelocityRig rig;
  recovery_core::RecoveryRegistry registry;

  {
    // NodeHandle chết trước registry: behavior vẫn phải sống, vì registry mới là thứ giữ .so.
    robot::NodeHandle scoped_nh;
    ASSERT_TRUE(registry.loadFromConfig(scoped_nh, "recovery", rig.ctx));
  }

  auto* wait = recovery_test::findBehavior(registry, "wait");
  ASSERT_NE(wait, nullptr);
  ASSERT_TRUE(wait->start(recovery_core::RecoveryGoal(), robot::Time(1000.0)));
  EXPECT_EQ(wait->update(robot::Time(1003.0)).status, recovery_core::RecoveryStatus::kSucceeded);
}

}  // namespace

int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
  setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
  setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}