optimal
This commit is contained in:
287
test/mission_adapter_bridge_test.cpp
Normal file
287
test/mission_adapter_bridge_test.cpp
Normal file
@@ -0,0 +1,287 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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, 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);
|
||||
|
||||
EXPECT_FALSE(request.tolerance.hasXy());
|
||||
EXPECT_FALSE(request.tolerance.hasYaw());
|
||||
}
|
||||
|
||||
// ================================================================================================
|
||||
// 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 đi thẳng xuống navigation, bỏ qua biên thread";
|
||||
|
||||
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()) << "cùng một mission bị đẩy xuống hai lần";
|
||||
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) << "mission cũ thắng mission mới";
|
||||
}
|
||||
|
||||
// ================================================================================================
|
||||
// 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) << "goal trực tiếp bị đem báo lên 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 không giao mission nào để chạy";
|
||||
|
||||
fixture.bridge_.reportOutcome(running->id, NavigationOutcome::kSucceeded);
|
||||
|
||||
EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U);
|
||||
EXPECT_EQ(manager.currentMissionId(), mission_adapters::kInvalidMissionId)
|
||||
<< "mission vẫn còn đang chạy sau khi đã báo hoàn tất";
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
Reference in New Issue
Block a user