optimal & fix file cmake
This commit is contained in:
368
test/mission_layer_test.cpp
Normal file
368
test/mission_layer_test.cpp
Normal file
@@ -0,0 +1,368 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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();
|
||||
}
|
||||
Reference in New Issue
Block a user