move_base2/test/planner_runner_test.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test PlannerRunner: nạp plugin thật qua Boost.DLL, và mọi đường lỗi phải trả false
 * chứ không được để dữ liệu hỏng đi tiếp.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <chrono>
#include <cstdlib>
#include <cstdint>
#include <limits>
#include <string>
#include <thread>
#include <vector>

#include <robot/node_handle.h>

#include <move_base2/runners/planner_runner.h>

namespace
{
using move_base2::PlannerRunner;

/**
 * @brief Con trỏ costmap giả.
 *
 * `PlannerRunner::configure` từ chối costmap null — đúng, vì mọi plugin thật đều dùng nó. Nhưng
 * `Costmap2DROBOT` không dựng được trong unit test (cần `tf3::BufferCore` thật và cây config đầy
 * đủ), nên test dùng một địa chỉ hợp lệ nhưng không phải costmap.
 *
 * An toàn ở đây vì `test_global_planner.cpp` **không alias nào chạm vào con trỏ này** — nó chỉ được
 * chuyển tiếp qua `initialize()` rồi bị bỏ qua. Đườ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);
}

/// @brief Chạy trọn một lượt lập plan đồng bộ hoá lại cho test: kick, chờ thread, lấy kết quả.
bool runOnePlan(move_base2::PlannerRunner& runner, const robot_geometry_msgs::PoseStamped& start,
                const robot_geometry_msgs::PoseStamped& goal,
                const robot_protocol_msgs::Order* order, move_base2::PlanResult& result)
{
  if (!runner.startPlan(start, goal, order, /*tag=*/1))
  {
    return false;
  }
  // Thread planner là thread thật; test phải chờ nó. Vòng quay ngắn thay vì sleep cố định để test
  // không phụ thuộc vào tốc độ máy.
  for (int i = 0; i < 10000 && runner.isPlanning(); ++i)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }
  return runner.pollPlan(result);
}

robot_geometry_msgs::PoseStamped makePose(double x, double y)
{
  robot_geometry_msgs::PoseStamped pose;
  pose.header.frame_id = "map";
  pose.pose.position.x = x;  // [m]
  pose.pose.position.y = y;  // [m]
  pose.pose.orientation.w = 1.0;
  return pose;
}

/// @brief Runner đã configure với planner @p name; ASSERT nếu không nạp được.
class Fixture
{
public:
  explicit Fixture(const std::string& name = "TestPlannerOk")
  {
    robot::NodeHandle nh;
    std::string error;
    ok_ = runner_.configure(nh, dummyCostmap(), name, error);
    error_ = error;
  }

  bool ok() const
  {
    return ok_;
  }

  const std::string& error() const
  {
    return error_;
  }

  PlannerRunner runner_;

private:
  bool ok_ = false;
  std::string error_;
};

}  // namespace

// ================================================================================================
// Cấu hình
// ================================================================================================

TEST(PlannerRunner, RefusesNullCostmap)
{
  // Plugin thật nào cũng dùng costmap. Nhận null rồi chuyển tiếp xuống `initialize()` là đẩy quyết
  // định "sập hay không" cho từng plugin tự lo.
  robot::NodeHandle nh;
  PlannerRunner runner;
  std::string error;

  EXPECT_FALSE(runner.configure(nh, nullptr, "TestPlannerOk", error));
  EXPECT_FALSE(error.empty());
  EXPECT_FALSE(runner.configured());
}

TEST(PlannerRunner, ConfiguresWithoutAnInitialPlanner)
{
  robot::NodeHandle nh;
  PlannerRunner runner;
  std::string error;

  ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "", error)) << error;
  EXPECT_TRUE(runner.configured());
  EXPECT_TRUE(runner.activePlanner().empty());
  EXPECT_EQ(runner.loadedCount(), 0U);
}

TEST(PlannerRunner, RefusesSecondConfigure)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  robot::NodeHandle nh;
  std::string error;
  EXPECT_FALSE(fixture.runner_.configure(nh, dummyCostmap(), "TestPlannerOk", error));
}

TEST(PlannerRunner, ConfigureFailsWhenTheInitialPlannerCannotBeLoaded)
{
  robot::NodeHandle nh;
  PlannerRunner runner;
  std::string error;

  EXPECT_FALSE(runner.configure(nh, dummyCostmap(), "TestPlannerMissingLibrary", error));
  EXPECT_FALSE(error.empty());
  EXPECT_FALSE(runner.configured()) << "configure failed but the object still reports itself as "
                                       "configured";
}

// ================================================================================================
// Nạp plugin qua Boost.DLL
// ================================================================================================

TEST(PlannerRunner, LoadsTheInitialPlannerAndReportsItAsActive)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
  EXPECT_EQ(fixture.runner_.loadedCount(), 1U);
}

TEST(PlannerRunner, SwapsBetweenPlannersAndReusesLoadedLibraries)
{
  // swapPlanner chạy ở CỬA VÀO mỗi yêu cầu. Đổi qua lại giữa hai profile không được dlopen lại.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  ASSERT_TRUE(fixture.runner_.swapPlanner("TestPlannerEmptyPlan"));
  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerEmptyPlan");
  EXPECT_EQ(fixture.runner_.loadedCount(), 2U);

  ASSERT_TRUE(fixture.runner_.swapPlanner("TestPlannerOk"));
  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "switched back to the previous planner yet "
                                                  "reloaded the library";
}

TEST(PlannerRunner, FailedSwapKeepsThePreviousPlannerActive)
{
  // Bên gọi từ chối yêu cầu dựa trên giá trị trả về. Chuyển sang trạng thái "không có planner" sẽ
  // giết luôn yêu cầu đang chạy dở, dù nó chẳng liên quan gì tới planner vừa nạp hỏng.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  EXPECT_FALSE(fixture.runner_.swapPlanner("TestPlannerMissingLibrary"));
  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");

  move_base2::PlanResult result;
  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result));
  EXPECT_TRUE(result.succeeded);
}

TEST(PlannerRunner, PlannerReportingInitializeFailureIsRejected)
{
  // Bản cũ chỉ log rồi đi tiếp với planner chưa khởi tạo xong.
  robot::NodeHandle nh;
  PlannerRunner runner;
  std::string error;

  EXPECT_FALSE(runner.configure(nh, dummyCostmap(), "TestPlannerInitFails", error));
}

TEST(PlannerRunner, PlannerThatFailedToInitializeIsNotCached)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  EXPECT_FALSE(fixture.runner_.swapPlanner("TestPlannerInitFails"));
  EXPECT_EQ(fixture.runner_.loadedCount(), 1U)
      << "a broken instance was cached — every later attempt would get that same broken one back";
}

TEST(PlannerRunner, RefusesEmptyPlannerName)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  EXPECT_FALSE(fixture.runner_.swapPlanner(""));
  EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
}

TEST(PlannerRunner, SwapBeforeConfigureIsRefused)
{
  PlannerRunner runner;
  EXPECT_FALSE(runner.swapPlanner("TestPlannerOk"));
}

// ================================================================================================
// Một lượt lập plan — mọi đường lỗi phải báo thất bại, plan phải rỗng
// ================================================================================================

TEST(PlannerRunner, ProducesANonEmptyPlanOnTheHappyPath)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  move_base2::PlanResult result;
  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 1.0), nullptr, result));
  ASSERT_TRUE(result.succeeded);
  ASSERT_FALSE(result.plan.empty());
  EXPECT_EQ(result.tag, 1U);
  EXPECT_DOUBLE_EQ(result.plan.back().pose.position.x, 2.0);
  EXPECT_DOUBLE_EQ(result.plan.back().pose.position.y, 1.0);
}

TEST(PlannerRunner, OrderIsForwardedToTheOrderAwareOverload)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  const robot_protocol_msgs::Order order;
  move_base2::PlanResult result;
  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), &order, result));
  EXPECT_TRUE(result.succeeded);
  EXPECT_FALSE(result.plan.empty());
}

TEST(PlannerRunner, OrderIsCopiedSoItMayDieBeforeThePlanFinishes)
{
  // Con trỏ Order chỉ hợp lệ trong lời gọi startPlan, nhưng lượt lập plan sống lâu hơn thế. Không
  // sao chép là thread planner đọc bộ nhớ đã chết.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  {
    const robot_protocol_msgs::Order order;
    ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), &order, 7));
  }  // order chết ở đây

  for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }

  move_base2::PlanResult result;
  ASSERT_TRUE(fixture.runner_.pollPlan(result));
  EXPECT_TRUE(result.succeeded);
  EXPECT_EQ(result.tag, 7U);
}

TEST(PlannerRunner, PlannerReturningTrueWithAnEmptyPlanIsTreatedAsFailure)
{
  // Contract của PlannerPort: thành công nghĩa là plan KHÔNG rỗng. Lọt qua thì tầng trên gọi
  // front()/back() trên vector rỗng.
  Fixture fixture("TestPlannerEmptyPlan");
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  move_base2::PlanResult result;
  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result));
  EXPECT_FALSE(result.succeeded);
  EXPECT_TRUE(result.plan.empty());
}

TEST(PlannerRunner, ExceptionFromThePluginIsContained)
{
  // Plugin là code bên thứ ba nạp lúc chạy. Exception thoát khỏi thân thread là std::terminate —
  // mất cả tiến trình navigation vì một lượt lập plan hỏng.
  Fixture fixture("TestPlannerThrowing");
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  move_base2::PlanResult result;
  ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result));
  EXPECT_FALSE(result.succeeded);
  EXPECT_TRUE(result.plan.empty());
}

TEST(PlannerRunner, NonFiniteStartOrGoalIsRejectedBeforeStartingTheThread)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  const double nan = std::numeric_limits<double>::quiet_NaN();
  const double inf = std::numeric_limits<double>::infinity();

  EXPECT_FALSE(fixture.runner_.startPlan(makePose(nan, 0.0), makePose(2.0, 0.0), nullptr, 1));
  EXPECT_FALSE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(inf, 0.0), nullptr, 1));
  EXPECT_FALSE(fixture.runner_.isPlanning());
}

TEST(PlannerRunner, StartPlanWithoutAnActivePlannerFails)
{
  robot::NodeHandle nh;
  PlannerRunner runner;
  std::string error;
  ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "", error)) << error;

  EXPECT_FALSE(runner.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1));
}

TEST(PlannerRunner, SecondStartWhileOneIsInFlightIsRefused)
{
  // Một lượt tại một thời điểm. Nhận thêm sẽ đè lên yêu cầu đang chạy và làm mất công đã bỏ ra.
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1));
  const bool refused = !fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(3.0, 0.0), nullptr, 2);

  for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }

  // Lượt đầu có thể đã xong trước lời gọi thứ hai (planner giả rất nhanh), nên chỉ khẳng định điều
  // luôn đúng: không bao giờ có hai lượt cùng chạy, và kết quả thu về là của MỘT lượt.
  move_base2::PlanResult result;
  ASSERT_TRUE(fixture.runner_.pollPlan(result));
  EXPECT_TRUE(result.tag == 1U || (!refused && result.tag == 2U));
  EXPECT_FALSE(fixture.runner_.pollPlan(result)) << "a second result is still sitting in the "
                                                    "mailbox";
}

TEST(PlannerRunner, CancelledPlanProducesNoResult)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1));
  fixture.runner_.cancelPlan();

  for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }

  move_base2::PlanResult result;
  EXPECT_FALSE(fixture.runner_.pollPlan(result))
      << "a cancelled attempt still returned a result — the caller would follow a plan to a goal "
         "nobody asks for anymore";
}

TEST(PlannerRunner, ResultCarriesBackTheTagItWasStartedWith)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 42));
  for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }

  move_base2::PlanResult result;
  ASSERT_TRUE(fixture.runner_.pollPlan(result));
  EXPECT_EQ(result.tag, 42U);
}

TEST(PlannerRunner, PollOnAnIdleRunnerReturnsNothing)
{
  Fixture fixture;
  ASSERT_TRUE(fixture.ok()) << fixture.error();

  move_base2::PlanResult result;
  EXPECT_FALSE(fixture.runner_.pollPlan(result));
  EXPECT_FALSE(fixture.runner_.isPlanning());
}

TEST(PlannerRunner, DestructorJoinsWhileAPlanIsInFlight)
{
  // Detach thay vì join sẽ để thread chạm vào buffer đã bị huỷ. Test này chạy sạch dưới sanitizer
  // là bằng chứng; ở đây nó ít nhất khẳng định destructor không treo.
  robot::NodeHandle nh;
  std::string error;
  {
    PlannerRunner runner;
    ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "TestPlannerOk", error)) << error;
    EXPECT_TRUE(runner.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1));
  }
  SUCCEED();
}

int main(int argc, char** argv)
{
  // ctest không mang theo biến môi trường của shell; binary tự trỏ vào cây config và thư viện của
  // gói, đúng cách recovery_runner_test và action_runner_test đang làm.
  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();
}