move_base2/test/mission_adapter_bridge_test.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test MissionAdapterBridge.
 *
 * Hai thứ đáng khoá lại: phần chuyển đổi Mission -> NavigationRequest, và **biên thread** — mission
 * layer đẩy chặng xuống từ thread của nó, nhưng chặng đó chỉ được chạm vào control loop trên control
 * thread.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <memory>
#include <string>
#include <vector>

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

#include <move_base2/bridges/mission_adapter_bridge.h>

namespace
{
using mission_adapters::Mission;
using mission_adapters::MissionManager;
using move_base2::MissionAdapterBridge;
using move_base2::NavigationOutcome;
using move_base2::NavigationRequest;

std::shared_ptr<Mission> makeMission(mission_adapters::MissionId id, double goal_x = 2.0,
                                     std::size_t actions = 0, bool has_goal = true)
{
  auto mission = std::make_shared<Mission>();
  mission->id = id;
  mission->has_goal = has_goal;
  mission->goal.header.frame_id = "map";
  mission->goal.pose.position.x = goal_x;  // [m]
  mission->goal.pose.orientation.w = 1.0;

  for (std::size_t i = 0; i < actions; ++i)
  {
    mission_adapters::Action action;
    action.sequenceId = static_cast<int>(i);
    action.action.actionType = "act_" + std::to_string(i);
    mission->actions.push_back(action);
  }
  return mission;
}

/// @brief Bridge đã start, kèm chỗ nhận yêu cầu để test đọc lại.
class Fixture
{
public:
  Fixture()
  {
    bridge_.setRequestCallback([this](const NavigationRequest& request) {
      received_.push_back(request);
    });
    bridge_.setCancelCallback([this]() { ++cancel_calls_; });
    bridge_.start();
  }

  MissionAdapterBridge bridge_;
  std::vector<NavigationRequest> received_;
  std::size_t cancel_calls_ = 0;
};

}  // namespace

// ================================================================================================
// Chuyển đổi Mission -> NavigationRequest
// ================================================================================================

TEST(MissionAdapterBridgeConversion, CarriesIdGoalAndActions)
{
  const auto mission = makeMission(7, 3.5, 2);
  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);

  EXPECT_EQ(request.mission_sequence_id, 7U);
  EXPECT_TRUE(request.has_goal);
  EXPECT_DOUBLE_EQ(request.goal.pose.position.x, 3.5);
  ASSERT_EQ(request.actions.size(), 2U);
  EXPECT_EQ(request.actions[0].actionType, "act_0");
  EXPECT_EQ(request.actions[1].actionType, "act_1");
}

TEST(MissionAdapterBridgeConversion, ActionOnlyMissionKeepsHasGoalFalse)
{
  // D8: mission chỉ-có-action. Navigation bỏ qua phần di chuyển và vào thẳng thực thi action.
  const auto mission = makeMission(9, 0.0, 3, /*has_goal=*/false);
  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);

  EXPECT_FALSE(request.has_goal);
  EXPECT_EQ(request.actions.size(), 3U);
}

TEST(MissionAdapterBridgeConversion, CarriesDockingMarkerIndependentlyOfGoalFrame)
{
  auto mission = makeMission(10);
  mission->motion_hint = "docking";
  mission->marker = "charger";
  mission->goal_frame = "charger_02_goal";

  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
  EXPECT_EQ(request.profile, move_base2::MotionProfile::kDocking);
  EXPECT_EQ(request.marker, "charger");
  EXPECT_EQ(request.goal_frame, "charger_02_goal");
}

TEST(MissionAdapterBridgeConversion, OrderLegCarriesItsOwnNodesAndEdges)
{
  // Global planner của profile position (`CustomPlanner`) CHỈ hiện thực nhánh
  // makePlan(Order, ...); nhánh ba tham số của nó là stub trả false. Chặng đi xuống mà không mang
  // order thì fail ngay lượt lập plan đầu và chạy thẳng vào recovery cho tới ABORTED — đã xảy ra
  // thật ngày 2026-07-31.
  auto mission = makeMission(4, 2.0);
  mission->type = mission_adapters::MissionType::VDA5050_ORDER;

  robot_protocol_msgs::Node n0;
  n0.nodeId = "n0";
  robot_protocol_msgs::Node n1;
  n1.nodeId = "n1";
  mission->nodes = { n0, n1 };

  robot_protocol_msgs::Edge e0;
  e0.edgeId = "e0";
  e0.startNodeId = "n0";
  e0.endNodeId = "n1";
  e0.trajectory.degree = 1;
  mission->edges = { e0 };

  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);

  ASSERT_NE(request.order, nullptr);
  ASSERT_EQ(request.order->nodes.size(), 2U);
  EXPECT_EQ(request.order->nodes[1].nodeId, "n1");

  // Edge của CHẶNG, không phải của cả order: planner tra edge theo startNodeId/endNodeId trong tập
  // node nó nhận được, nên tập hai bên phải khớp nhau.
  ASSERT_EQ(request.order->edges.size(), 1U);
  EXPECT_EQ(request.order->edges[0].startNodeId, "n0");
  EXPECT_EQ(request.order->edges[0].trajectory.degree, 1U);
}

TEST(MissionAdapterBridgeConversion, SimpleGoalMissionCarriesNoOrder)
{
  const auto mission = makeMission(2);  // mặc định SIMPLE_GOAL
  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
  EXPECT_EQ(request.order, nullptr);
}

TEST(MissionAdapterBridgeConversion, ActionOnlyLegCarriesNoOrder)
{
  // Không có quãng đường nào để lập plan, nên cũng không có gì để đưa cho planner.
  auto mission = makeMission(3, 0.0, 1, /*has_goal=*/false);
  mission->type = mission_adapters::MissionType::VDA5050_ORDER;

  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
  EXPECT_EQ(request.order, nullptr);
}

TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault)
{
  // Quy ước của NavigationRequest: sai số <= 0 nghĩa "dùng default của profile trong config".
  // Mission layer không biết gì về sai số hình học nên không được đặt.
  const auto mission = makeMission(1);
  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);

}

// ================================================================================================
// Biên thread: dispatch chỉ cất lại, control thread mới đẩy xuống
// ================================================================================================

TEST(MissionAdapterBridge, DispatchDoesNotReachNavigationUntilPumped)
{
  // `MissionExecutor` gọi dispatch từ thread của nó, còn ControlLoop không thread-safe. Đẩy thẳng
  // xuống ở đây nghĩa là hai thread cùng ghi pending_request_ của control loop.
  Fixture fixture;

  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(3)));
  EXPECT_TRUE(fixture.received_.empty()) << "dispatch went straight down to navigation, skipping "
                                            "the thread boundary";

  EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
  ASSERT_EQ(fixture.received_.size(), 1U);
  EXPECT_EQ(fixture.received_[0].mission_sequence_id, 3U);
}

TEST(MissionAdapterBridge, PumpingWithNothingPendingIsANoOp)
{
  Fixture fixture;

  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest());
  EXPECT_TRUE(fixture.received_.empty());
}

TEST(MissionAdapterBridge, EachMissionIsPushedDownExactlyOnce)
{
  Fixture fixture;

  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(4)));
  EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "the same mission was pushed down twice";
  EXPECT_EQ(fixture.received_.size(), 1U);
}

TEST(MissionAdapterBridge, DispatchBeforeStartIsRefused)
{
  // Mission layer phải biết chặng của nó không được nhận, chứ không phải chờ một kết quả sẽ không
  // bao giờ tới.
  MissionAdapterBridge bridge;
  EXPECT_FALSE(bridge.dispatch(makeMission(1)));
}

TEST(MissionAdapterBridge, DispatchAfterStopIsRefused)
{
  Fixture fixture;
  fixture.bridge_.stop();

  EXPECT_FALSE(fixture.bridge_.dispatch(makeMission(1)));
  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest());
}

TEST(MissionAdapterBridge, StopDropsAMissionThatWasStillWaiting)
{
  // Giữ lại để nó chạy sau một lần start() nào đó về sau là hành vi không ai mong đợi.
  Fixture fixture;
  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(5)));

  fixture.bridge_.stop();
  fixture.bridge_.start();

  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest());
  EXPECT_TRUE(fixture.received_.empty());
}

TEST(MissionAdapterBridge, NullMissionIsRefused)
{
  Fixture fixture;
  EXPECT_FALSE(fixture.bridge_.dispatch(nullptr));
}

TEST(MissionAdapterBridge, OverwritingAWaitingMissionIsCounted)
{
  // Không nên xảy ra — MissionManager chỉ giao chặng mới sau khi chặng cũ kết thúc. Nhưng im lặng ở
  // đây nghĩa là một chặng biến mất trong khi fleet master vẫn chờ nó.
  Fixture fixture;

  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(1)));
  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(2)));

  EXPECT_EQ(fixture.bridge_.droppedRequests(), 1U);

  ASSERT_TRUE(fixture.bridge_.pumpPendingRequest());
  ASSERT_EQ(fixture.received_.size(), 1U);
  EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "the old mission won over the new one";
}

// ================================================================================================
// Huỷ
// ================================================================================================

TEST(MissionAdapterBridge, CancelReachesNavigationThroughTheCallback)
{
  Fixture fixture;
  fixture.bridge_.cancelActive(1);

  EXPECT_EQ(fixture.cancel_calls_, 1U);
}

TEST(MissionAdapterBridge, CancelDropsAMissionThatHadNotBeenPushedDownYet)
{
  // Đẩy nó xuống rồi mới huỷ là cho robot nhúc nhích một cycle vì một chặng đã bị thu hồi.
  Fixture fixture;
  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(6)));

  fixture.bridge_.cancelActive(6);

  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest());
  EXPECT_TRUE(fixture.received_.empty());
}

// ================================================================================================
// Báo kết quả về mission layer
// ================================================================================================

TEST(MissionAdapterBridge, ReportingWithoutAManagerIsSafe)
{
  // Chạy không có mission layer là cấu hình hợp lệ: goal có thể đến thẳng từ contract host.
  Fixture fixture;
  EXPECT_NO_THROW(fixture.bridge_.reportOutcome(1, NavigationOutcome::kSucceeded));
}

TEST(MissionAdapterBridge, DirectGoalWithoutMissionIdIsNotReported)
{
  MissionManager manager;
  Fixture fixture;
  fixture.bridge_.attach(&manager);

  fixture.bridge_.reportOutcome(0, NavigationOutcome::kSucceeded);

  EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "a direct goal was reported up to the mission "
                                                    "layer";
}

TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone)
{
  MissionManager manager;
  Fixture fixture;
  fixture.bridge_.attach(&manager);

  manager.submit({ makeMission(0) });
  const auto running = manager.nextMission();
  ASSERT_TRUE(running) << "manager handed over no mission to run";

  fixture.bridge_.reportOutcome(running->id, NavigationOutcome::kSucceeded);

  EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U);
  EXPECT_EQ(manager.currentMissionId(), mission_adapters::kInvalidMissionId)
      << "mission is still running after completion was reported";
}

TEST(MissionAdapterBridge, OutcomeForAMissionThatIsNoLongerRunningIsCounted)
{
  // Outcome tới trễ sau khi mission đã bị thay. Manager từ chối nó — đúng ý, nhưng phải đếm được,
  // vì đếm tăng bất thường nghĩa là bất biến "báo đúng một lần" đang bị vi phạm ở đâu đó.
  MissionManager manager;
  Fixture fixture;
  fixture.bridge_.attach(&manager);

  fixture.bridge_.reportOutcome(9999, NavigationOutcome::kSucceeded);

  EXPECT_EQ(fixture.bridge_.staleOutcomes(), 1U);
}

TEST(MissionAdapterBridge, HasActiveMissionCountsAMissionStillWaitingToBePushedDown)
{
  Fixture fixture;
  EXPECT_FALSE(fixture.bridge_.hasActiveMission());

  ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(2)));
  EXPECT_TRUE(fixture.bridge_.hasActiveMission());
}

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