move_base2/test/mission_layer_test.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test MissionLayer lắp với MissionAdapterBridge thật.
 *
 * Đây là chỗ kiểm thứ mà `mission_adapter_bridge_test` không kiểm được: bridge có được nối vào một
 * mission layer ĐANG CHẠY hay không, và một yêu cầu nhiều chặng có thật sự đi hết chặng này tới
 * chặng kia hay không. Bridge đúng mà layer không được dựng thì mọi test bridge vẫn xanh trong khi
 * robot chỉ chạy được chặng đầu — đúng trạng thái của gói trước lần sửa này.
 *
 * Nguồn mission ở đây được đăng ký thẳng vào registry (không qua Boost.DLL): đường nạp `.so` đã có
 * `plugin_registry_test` của `mission_adapters` lo, còn thứ cần khoá tại đây là chuỗi sự kiện.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include <vector>

#include <mission_adapters/mission_request.h>
#include <mission_adapters/types.h>

#include <move_base2/bridges/mission_layer.h>

namespace
{
using mission_adapters::ConversionResult;
using mission_adapters::Mission;
using mission_adapters::MissionRequest;
using mission_adapters::MissionSourceAdapter;
using mission_adapters::MissionState;
using mission_adapters::SubmitMode;
using move_base2::MissionAdapterBridge;
using move_base2::MissionLayer;
using move_base2::NavigationOutcome;
using move_base2::NavigationRequest;

constexpr auto kTimeout = std::chrono::seconds(2);
constexpr auto kPollStep = std::chrono::milliseconds(2);

/**
 * @brief Nguồn giả cắt một pose thành @c legs chặng, mô phỏng đúng hình dạng của một VDA5050 order
 *        nhiều node có action.
 */
class SplittingAdapter : public MissionSourceAdapter
{
public:
  explicit SplittingAdapter(std::size_t legs, SubmitMode mode = SubmitMode::kReplace)
    : legs_(legs), mode_(mode)
  {
  }

  bool configure(const std::string&, robot::NodeHandle&) override
  {
    return true;
  }

  std::string schema() const override
  {
    return mission_adapters::schema::kPoseStamped;
  }

  bool validate(const MissionRequest& request, std::string& reason) const override
  {
    if (!request.pose)
    {
      reason = "request carries no pose";
      return false;
    }
    return true;
  }

  ConversionResult convert(const MissionRequest& request) override
  {
    ConversionResult result;
    result.mode = mode_;
    for (std::size_t i = 0; i < legs_; ++i)
    {
      auto mission = std::make_shared<Mission>();
      mission->has_goal = true;
      mission->goal = *request.pose;
      // Mỗi chặng xa hơn chặng trước một mét — đủ để test phân biệt được chặng nào đang chạy.
      mission->goal.pose.position.x = static_cast<double>(i + 1);  // [m]
      result.missions.push_back(mission);
    }
    return result;
  }

private:
  std::size_t legs_;
  SubmitMode mode_;
};

robot_geometry_msgs::PoseStamped makeGoal()
{
  robot_geometry_msgs::PoseStamped goal;
  goal.header.frame_id = "map";
  goal.pose.orientation.w = 1.0;
  return goal;
}

/// @brief Layer + bridge đã nối, cộng chỗ nhận yêu cầu như control thread thật.
class Fixture
{
public:
  explicit Fixture(std::size_t legs, SubmitMode mode = SubmitMode::kReplace)
  {
    adapter_ = std::make_shared<SplittingAdapter>(legs, mode);
    EXPECT_TRUE(layer_.registry().registerAdapter(adapter_));
    layer_.markActiveForTesting();
    layer_.attach(bridge_);

    bridge_.setRequestCallback([this](const NavigationRequest& request) {
      received_.push_back(request);
    });
    bridge_.setCancelCallback([this]() { ++cancel_calls_; });

    bridge_.start();
    layer_.start();
  }

  ~Fixture()
  {
    layer_.stop();
    bridge_.stop();
  }

  /**
   * @brief Quay control thread cho tới khi một chặng được đẩy xuống, hoặc hết thời gian chờ.
   * @return false nếu không có chặng nào tới — dùng để khẳng định "KHÔNG được có chặng mới".
   */
  bool pumpUntilRequest()
  {
    const auto deadline = std::chrono::steady_clock::now() + kTimeout;
    while (std::chrono::steady_clock::now() < deadline)
    {
      if (bridge_.pumpPendingRequest())
      {
        return true;
      }
      std::this_thread::sleep_for(kPollStep);
    }
    return false;
  }

  /// @brief Chờ mission layer đạt tới trạng thái mong đợi.
  bool waitForState(MissionState expected)
  {
    const auto deadline = std::chrono::steady_clock::now() + kTimeout;
    while (std::chrono::steady_clock::now() < deadline)
    {
      if (layer_.state() == expected)
      {
        return true;
      }
      std::this_thread::sleep_for(kPollStep);
    }
    return false;
  }

  /// @brief Báo kết cục của chặng vừa nhận, đúng như ControlLoop làm ở cuối một phiên.
  void finishLastLeg(NavigationOutcome outcome)
  {
    ASSERT_FALSE(received_.empty());
    bridge_.reportOutcome(received_.back().mission_sequence_id, outcome);
  }

  MissionLayer layer_;
  MissionAdapterBridge bridge_;
  std::vector<NavigationRequest> received_;
  int cancel_calls_ = 0;
  std::shared_ptr<SplittingAdapter> adapter_;
};

// ================================================================================================
// Chuỗi nhiều chặng — lý do lớp này tồn tại
// ================================================================================================

TEST(MissionLayerTest, ThreeLegOrderReachesNavigationOneLegAtATime)
{
  Fixture fixture(3);

  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));

  // Chặng 1
  ASSERT_TRUE(fixture.pumpUntilRequest());
  ASSERT_EQ(1u, fixture.received_.size());
  EXPECT_DOUBLE_EQ(1.0, fixture.received_[0].goal.pose.position.x);
  EXPECT_NE(0u, fixture.received_[0].mission_sequence_id);

  // Chặng 2 chỉ được giao SAU khi chặng 1 báo xong: hàng đợi tuần tự, không phải bắn hết một lượt.
  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
  ASSERT_TRUE(fixture.pumpUntilRequest());
  ASSERT_EQ(2u, fixture.received_.size());
  EXPECT_DOUBLE_EQ(2.0, fixture.received_[1].goal.pose.position.x);

  // Chặng 3
  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
  ASSERT_TRUE(fixture.pumpUntilRequest());
  ASSERT_EQ(3u, fixture.received_.size());
  EXPECT_DOUBLE_EQ(3.0, fixture.received_[2].goal.pose.position.x);

  // Mỗi chặng một id riêng: outcome trễ của chặng cũ không thể được tính cho chặng mới.
  EXPECT_NE(fixture.received_[0].mission_sequence_id, fixture.received_[1].mission_sequence_id);
  EXPECT_NE(fixture.received_[1].mission_sequence_id, fixture.received_[2].mission_sequence_id);

  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
  EXPECT_TRUE(fixture.waitForState(MissionState::COMPLETED));
  EXPECT_FALSE(fixture.layer_.hasMission());
}

TEST(MissionLayerTest, MissionStillPendingBetweenLegs)
{
  Fixture fixture(2);
  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
  ASSERT_TRUE(fixture.pumpUntilRequest());

  // Đây là tín hiệu mà NavigationServer dùng để KHÔNG báo SUCCEEDED cho host giữa hai chặng. Sai ở
  // đây nghĩa là fleet master nghe "đã tới node cuối" khi robot mới đi được nửa tuyến.
  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
  EXPECT_TRUE(fixture.bridge_.hasActiveMission());

  ASSERT_TRUE(fixture.pumpUntilRequest());
  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
  EXPECT_TRUE(fixture.waitForState(MissionState::COMPLETED));
  EXPECT_FALSE(fixture.bridge_.hasActiveMission());
}

// ================================================================================================
// Đường lỗi và đường huỷ
// ================================================================================================

TEST(MissionLayerTest, FailedLegClearsTheRestOfTheQueue)
{
  Fixture fixture(3);
  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
  ASSERT_TRUE(fixture.pumpUntilRequest());

  // clear_queue_on_failure mặc định true: không tới được node n thì chạy tiếp chặng n+1 là cắt ngang
  // đoạn đường fleet manager chưa cho phép đi.
  fixture.finishLastLeg(NavigationOutcome::kFailed);
  EXPECT_TRUE(fixture.waitForState(MissionState::FAILED));
  EXPECT_FALSE(fixture.pumpUntilRequest());
  EXPECT_EQ(1u, fixture.received_.size());
}

TEST(MissionLayerTest, CancelStopsNavigationAndDropsTheQueue)
{
  Fixture fixture(3);
  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
  ASSERT_TRUE(fixture.pumpUntilRequest());

  fixture.layer_.cancel();
  EXPECT_TRUE(fixture.waitForState(MissionState::CANCELLED));

  // Huỷ phải dừng được robot, không chỉ xoá hàng đợi trong bộ nhớ (A3).
  const auto deadline = std::chrono::steady_clock::now() + kTimeout;
  while (fixture.cancel_calls_ == 0 && std::chrono::steady_clock::now() < deadline)
  {
    std::this_thread::sleep_for(kPollStep);
  }
  EXPECT_GE(fixture.cancel_calls_, 1);

  EXPECT_FALSE(fixture.pumpUntilRequest());
  EXPECT_EQ(1u, fixture.received_.size());
}

TEST(MissionLayerTest, PausedLayerHoldsTheQueueUntilResume)
{
  Fixture fixture(2);

  fixture.layer_.pause();
  ASSERT_TRUE(fixture.waitForState(MissionState::PAUSED));

  // Yêu cầu tới trong lúc người vận hành đang chủ động dừng: nhận vào hàng đợi nhưng KHÔNG tự chạy.
  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
  EXPECT_FALSE(fixture.pumpUntilRequest());
  EXPECT_TRUE(fixture.received_.empty());

  fixture.layer_.resume();
  EXPECT_TRUE(fixture.pumpUntilRequest());
  EXPECT_EQ(1u, fixture.received_.size());
}

// ================================================================================================
// Định tuyến: layer chỉ nhận thứ nó có nguồn để xử lý
// ================================================================================================

TEST(MissionLayerTest, RejectsSchemaWithNoRegisteredSource)
{
  Fixture fixture(1);

  // Không có nguồn nào khai schema `vda5050.order` — layer phải TỪ CHỐI để NavigationServer biết
  // đường mà rơi về nhánh trực tiếp, thay vì nuốt order rồi im lặng.
  robot_protocol_msgs::Order order;
  EXPECT_FALSE(fixture.layer_.handles(mission_adapters::schema::kVda5050Order));
  EXPECT_FALSE(fixture.layer_.submitOrder(order));
}

TEST(MissionLayerTest, RejectsEverythingBeforeStartAndAfterStop)
{
  MissionLayer layer;
  MissionAdapterBridge bridge;
  auto adapter = std::make_shared<SplittingAdapter>(1);
  ASSERT_TRUE(layer.registry().registerAdapter(adapter));
  layer.markActiveForTesting();
  layer.attach(bridge);

  // Chưa start: không được nhận việc mà sẽ không ai chạy.
  EXPECT_FALSE(layer.submitGoal(makeGoal()));

  bridge.start();
  layer.start();
  EXPECT_TRUE(layer.submitGoal(makeGoal()));

  layer.stop();
  bridge.stop();
  EXPECT_FALSE(layer.submitGoal(makeGoal()));
}

TEST(MissionLayerTest, InactiveLayerNeverStarts)
{
  // Không nguồn nào -> configure sẽ hỏng ở runtime thật; ở đây kiểm bất biến tương ứng: layer không
  // active thì start() là no-op và mọi đường vào đều đóng.
  MissionLayer layer;
  MissionAdapterBridge bridge;
  layer.attach(bridge);
  layer.start();

  EXPECT_FALSE(layer.active());
  EXPECT_FALSE(layer.started());
  EXPECT_EQ(0u, layer.sourceCount());
  EXPECT_FALSE(layer.submitGoal(makeGoal()));
}

// ================================================================================================
// Order update — phần release thêm nối tiếp, không chạy lại từ đầu
// ================================================================================================

TEST(MissionLayerTest, AppendModeDoesNotPreemptTheRunningLeg)
{
  Fixture fixture(1, SubmitMode::kAppend);
  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
  ASSERT_TRUE(fixture.pumpUntilRequest());
  const std::uint64_t running_id = fixture.received_.back().mission_sequence_id;

  // Bản cập nhật tới trong lúc chặng cũ đang chạy: nó phải nằm chờ, không được huỷ chặng đang đi.
  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
  EXPECT_FALSE(fixture.pumpUntilRequest());
  EXPECT_EQ(0, fixture.cancel_calls_);

  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
  ASSERT_TRUE(fixture.pumpUntilRequest());
  EXPECT_NE(running_id, fixture.received_.back().mission_sequence_id);
}

}  // namespace

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}