diff --git a/CMakeLists.txt b/CMakeLists.txt index fc8dce2..95e1ed6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -92,7 +92,7 @@ else() catkin_package( INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_utils + LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS robot_costmap_2d robot_nav_core @@ -105,7 +105,7 @@ else() robot_visualization_msgs robot_nav_2d_utils data_convert - DEPENDS PCL Boost Eigen + DEPENDS PCL Boost ) include_directories( @@ -218,11 +218,11 @@ endif() # ======================================================== if(BUILDING_WITH_CATKIN) - # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils - # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - # ) + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} @@ -237,12 +237,12 @@ if(BUILDING_WITH_CATKIN) else() - # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils - # EXPORT ${PROJECT_NAME}-targets - # ARCHIVE DESTINATION lib - # LIBRARY DESTINATION lib - # RUNTIME DESTINATION bin - # ) + install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) install(EXPORT ${PROJECT_NAME}-targets FILE ${PROJECT_NAME}-targets.cmake @@ -290,24 +290,15 @@ if(CATKIN_ENABLE_TESTING) message(FATAL_ERROR "catkin_add_gtest NOT FOUND") endif() - find_package(GTest REQUIRED) - - add_executable(test_mission_adapters - src/test_mission_adapters.cpp - ) - - target_link_libraries(test_mission_adapters - mission_adapters - GTest::GTest - GTest::Main - pthread - ${catkin_LIBRARIES} + catkin_add_gtest(test_mission_adapters + test/mission_adapters_test.cpp ) if(TARGET test_mission_adapters) target_link_libraries(test_mission_adapters mission_adapters ${catkin_LIBRARIES} + pthread ) target_include_directories(test_mission_adapters PRIVATE diff --git a/include/mission_adapters/mission_adapters.h b/include/mission_adapters/mission_adapters.h index 4b0f057..6986ef8 100644 --- a/include/mission_adapters/mission_adapters.h +++ b/include/mission_adapters/mission_adapters.h @@ -7,8 +7,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -68,17 +70,17 @@ namespace mission_adapters class Action { public: - int sequenceId; - ActionType type; + int sequenceId = 0; + ActionType type = ActionType::NODE_ACTION; robot_protocol_msgs::Action action; }; class Mission { public: - int sequenceId; - MissionType type; - int priority; + int sequenceId = 0; + MissionType type = MissionType::SIMPLE_GOAL; + int priority = 0; robot_geometry_msgs::PoseStamped start; robot_geometry_msgs::PoseStamped goal; std::vector nodes; @@ -88,8 +90,9 @@ namespace mission_adapters struct Event { - EventType type; - int priority; + EventType type = EventType::SUBMIT_MISSIONS; + int priority = PRIORITY_ORDER; + uint64_t sequence = 0; std::vector> missions; }; @@ -97,7 +100,10 @@ namespace mission_adapters { bool operator()(const Event& lhs, const Event& rhs) const { - return lhs.priority > rhs.priority; + if (lhs.priority != rhs.priority) + return lhs.priority > rhs.priority; + + return lhs.sequence > rhs.sequence; } }; @@ -108,6 +114,7 @@ namespace mission_adapters std::priority_queue, EventCompare> queue_; std::condition_variable cv_; bool stop_ = false; + uint64_t next_sequence_ = 0; public: void push(const Event& event); @@ -235,4 +242,4 @@ namespace mission_adapters } // namespace mission_adapters -#endif // MISSION_ADAPTERS_MISSION_ADAPTERS_H \ No newline at end of file +#endif // MISSION_ADAPTERS_MISSION_ADAPTERS_H diff --git a/src/mission_adapters.cpp b/src/mission_adapters.cpp index 3a20c46..29b7532 100644 --- a/src/mission_adapters.cpp +++ b/src/mission_adapters.cpp @@ -11,13 +11,16 @@ namespace mission_adapters void EventBus::push(const Event& event) { std::lock_guard lock(mutex_); - queue_.push(event); + Event queued_event = event; + queued_event.sequence = next_sequence_++; + queue_.push(std::move(queued_event)); cv_.notify_one(); } void EventBus::push(Event&& event) { std::lock_guard lock(mutex_); + event.sequence = next_sequence_++; queue_.push(std::move(event)); cv_.notify_one(); } @@ -47,6 +50,8 @@ namespace mission_adapters { std::lock_guard lock(mutex_); stop_ = false; + next_sequence_ = 0; + while (!queue_.empty()) queue_.pop(); } // ───────────────────────────────────────────────────────────────────────── @@ -74,7 +79,13 @@ namespace mission_adapters std::vector> missions; std::vector action_node_indices; - if(order.nodes.empty()) return missions; + if (order.nodes.empty()) return missions; + + if (order.nodes.size() > 1 && order.edges.size() < order.nodes.size() - 1) + { + robot::log_error("Invalid VDA5050 order: edge count is smaller than node_count - 1"); + return missions; + } for (size_t i = 0; i < order.nodes.size(); ++i) { @@ -169,6 +180,10 @@ namespace mission_adapters void MissionManager::submit(const std::vector>& missions) { std::lock_guard lock(mutex_); + + if (state_ == MissionState::EMERGENCY) + return; + if (!mission_queue_.empty()) { mission_queue_ = {}; @@ -188,7 +203,6 @@ namespace mission_adapters case MissionState::COMPLETED: case MissionState::FAILED: case MissionState::CANCELLED: - case MissionState::EMERGENCY: case MissionState::CLEAR_EMERGENCY: state_ = MissionState::QUEUED; break; @@ -230,6 +244,10 @@ namespace mission_adapters void MissionManager::onNavigationDone() { std::lock_guard lock(mutex_); + + if (state_ != MissionState::RUNNING) + return; + current_mission_.reset(); state_ = mission_queue_.empty() ? MissionState::IDLE : MissionState::QUEUED; } @@ -237,6 +255,10 @@ namespace mission_adapters void MissionManager::onNavigationFailed() { std::lock_guard lock(mutex_); + + if (state_ != MissionState::RUNNING) + return; + current_mission_.reset(); while (!mission_queue_.empty()) mission_queue_.pop(); state_ = MissionState::FAILED; @@ -374,38 +396,59 @@ namespace mission_adapters void EventProcessor::pauseEvent() { - event_bus_.push({EventType::PAUSE, PRIORITY_PAUSE, {}}); + Event event; + event.type = EventType::PAUSE; + event.priority = PRIORITY_PAUSE; + event_bus_.push(std::move(event)); } void EventProcessor::resumeEvent() { - event_bus_.push({EventType::RESUME, PRIORITY_RESUME, {}}); + Event event; + event.type = EventType::RESUME; + event.priority = PRIORITY_RESUME; + event_bus_.push(std::move(event)); } void EventProcessor::cancelEvent() { - event_bus_.push({EventType::CANCEL, PRIORITY_CANCEL, {}}); + Event event; + event.type = EventType::CANCEL; + event.priority = PRIORITY_CANCEL; + event_bus_.push(std::move(event)); } void EventProcessor::navDoneEvent() { - event_bus_.push({EventType::NAV_DONE, PRIORITY_NAV_DONE, {}}); + Event event; + event.type = EventType::NAV_DONE; + event.priority = PRIORITY_NAV_DONE; + event_bus_.push(std::move(event)); } // FIX #4: Use PRIORITY_NAV_FAILED (correct constant name). void EventProcessor::navFailedEvent() { - event_bus_.push({EventType::NAV_FAILED, PRIORITY_NAV_FAILED, {}}); + Event event; + event.type = EventType::NAV_FAILED; + event.priority = PRIORITY_NAV_FAILED; + event_bus_.push(std::move(event)); } void EventProcessor::emergencyEvent() { - event_bus_.push({EventType::EMERGENCY, PRIORITY_EMERGENCY, {}}); + Event event; + event.type = EventType::EMERGENCY; + event.priority = PRIORITY_EMERGENCY; + event_bus_.push(std::move(event)); } void EventProcessor::clearEmergencyEvent() { - event_bus_.push({EventType::CLEAR_EMERGENCY, PRIORITY_EMERGENCY, {}}); + Event event; + event.type = EventType::CLEAR_EMERGENCY; + event.priority = PRIORITY_EMERGENCY; + event_bus_.push(std::move(event)); } // ───────────────────────────────────────────────────────────────────────── @@ -483,4 +526,4 @@ namespace mission_adapters } } -} // namespace mission_adapters \ No newline at end of file +} // namespace mission_adapters diff --git a/src/robot_control_test.cpp b/src/robot_control_test.cpp index bd80e67..be607ee 100644 --- a/src/robot_control_test.cpp +++ b/src/robot_control_test.cpp @@ -1,6 +1,8 @@ #include #include +#include + using namespace mission_adapters; class RobotControlTest @@ -23,7 +25,7 @@ class RobotControlTest } public: - RobotControlTest(); + explicit RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base); ~RobotControlTest(); void run(); @@ -32,7 +34,8 @@ private: void executeMission(const Mission& mission); }; -RobotControlTest::RobotControlTest() +RobotControlTest::RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base) + : move_base_ptr_(std::move(move_base)) {} RobotControlTest::~RobotControlTest() @@ -43,31 +46,43 @@ RobotControlTest::~RobotControlTest() void RobotControlTest::run() { + if (!move_base_ptr_) + { + robot::log_error("RobotControlTest requires a valid BaseNavigation pointer"); + return; + } + robot::Rate rate(50); - // FIX #1: Set the callback ONCE before starting the executor, not inside the loop. mission_executor_.setMissionCallback( [this](const std::shared_ptr& mission) { executeMission(*mission); }); - + event_processor_.start(); + mission_executor_.start(); - while (true) + while (robot::ok()) { auto feedback = move_base_ptr_->getFeedback(); + if (!feedback) + { + rate.sleep(); + continue; + } + auto nav_state = feedback->navigation_state; - if (nav_state != prev_nav_done_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone()) + if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone()) { event_processor_.navDoneEvent(); } - else if (nav_state == robot::move_base_core::State::ABORTED) + else if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::ABORTED) { event_processor_.navFailedEvent(); } - prev_nav_state_ = nav_state; + prev_nav_state_ = nav_state; // Example: receive an order (replace condition with your real source) if (/* new order available */ false) @@ -86,10 +101,3 @@ void RobotControlTest::executeMission(const Mission& mission) // TODO: send mission goal to move_base_ptr_ (void)mission; } - -int main() -{ - RobotControlTest test; - test.run(); - return 0; -} \ No newline at end of file diff --git a/src/test_mission_adapters b/src/test_mission_adapters deleted file mode 100755 index 80f4a89..0000000 Binary files a/src/test_mission_adapters and /dev/null differ diff --git a/src/test_mission_adapters.cpp b/src/test_mission_adapters.cpp deleted file mode 100755 index 0de5e50..0000000 --- a/src/test_mission_adapters.cpp +++ /dev/null @@ -1,578 +0,0 @@ -#include - -#include -#include -#include - -#include "mission_adapters/mission_adapters.h" - -using namespace mission_adapters; - -namespace -{ - -robot_geometry_msgs::PoseStamped MakeGoal(double x, double y) -{ - robot_geometry_msgs::PoseStamped g; - g.pose.position.x = x; - g.pose.position.y = y; - return g; -} - -robot_protocol_msgs::Action MakeAction( - const std::string &id) -{ - robot_protocol_msgs::Action a; - a.actionId = id; - a.actionType = "TEST"; - return a; -} - -robot_protocol_msgs::Node MakeNode( - int seq, - bool add_action = false) -{ - robot_protocol_msgs::Node n; - n.sequenceId = seq; - n.nodeId = "node_" + std::to_string(seq); - - n.nodePosition.x = seq; - n.nodePosition.y = seq; - - if (add_action) - n.actions.push_back( - MakeAction("node_action_" + std::to_string(seq))); - - return n; -} - -robot_protocol_msgs::Edge MakeEdge( - int seq, - bool add_action = false) -{ - robot_protocol_msgs::Edge e; - - e.sequenceId = seq; - e.edgeId = "edge_" + std::to_string(seq); - - if (add_action) - e.actions.push_back( - MakeAction("edge_action_" + std::to_string(seq))); - - return e; -} - -robot_protocol_msgs::Order MakeOrder( - int node_count) -{ - robot_protocol_msgs::Order order; - - for (int i = 0; i < node_count; ++i) - order.nodes.push_back( - MakeNode(i)); - - for (int i = 0; i < node_count - 1; ++i) - order.edges.push_back( - MakeEdge(i)); - - return order; -} - -} // namespace - -//-------------------------------------------------------------- -// EventBus -//-------------------------------------------------------------- - -TEST(EventBus, PriorityOrdering) -{ - EventBus bus; - - bus.push({EventType::PAUSE, - PRIORITY_PAUSE, - {}}); - - bus.push({EventType::CANCEL, - PRIORITY_CANCEL, - {}}); - - bus.push({EventType::EMERGENCY, - PRIORITY_EMERGENCY, - {}}); - - Event e; - - ASSERT_TRUE(bus.pop(e)); - EXPECT_EQ(e.priority, - PRIORITY_EMERGENCY); - - ASSERT_TRUE(bus.pop(e)); - EXPECT_EQ(e.priority, - PRIORITY_CANCEL); - - ASSERT_TRUE(bus.pop(e)); - EXPECT_EQ(e.priority, - PRIORITY_PAUSE); -} - -TEST(EventBus, StopReturnsFalse) -{ - EventBus bus; - - std::thread t([&]() - { bus.stop(); }); - - Event e; - - EXPECT_FALSE(bus.pop(e)); - - t.join(); -} - -TEST(EventBus, ConcurrentPush) -{ - EventBus bus; - - constexpr int N = 100; - - std::thread producer([&]() - { - for (int i = 0; i < N; i++) - { - bus.push( - {EventType::NAV_DONE, - PRIORITY_NAV_DONE, - {}}); - } - }); - - int count = 0; - - while (count < N) - { - Event e; - - if (bus.pop(e)) - ++count; - } - - producer.join(); - - EXPECT_EQ(count, N); -} - -//-------------------------------------------------------------- -// GoalAdapter -//-------------------------------------------------------------- - -TEST(GoalAdapter, ConvertSingleGoal) -{ - GoalAdapter adapter; - - auto missions = - adapter.convert( - MakeGoal(5.5, 9.1)); - - ASSERT_EQ(missions.size(), 1u); - - EXPECT_EQ( - missions[0]->type, - MissionType::SIMPLE_GOAL); - - EXPECT_DOUBLE_EQ( - missions[0]->goal.pose.position.x, - 5.5); - - EXPECT_DOUBLE_EQ( - missions[0]->goal.pose.position.y, - 9.1); -} - -//-------------------------------------------------------------- -// VDA5050Adapter -//-------------------------------------------------------------- - -TEST(VDA5050Adapter, EmptyOrder) -{ - VDA5050Adapter adapter; - - robot_protocol_msgs::Order order; - - auto missions = - adapter.convert(order); - - EXPECT_TRUE( - missions.empty()); -} - -TEST(VDA5050Adapter, OrderWithoutActionsCreatesTailMission) -{ - VDA5050Adapter adapter; - - auto order = - MakeOrder(4); - - auto missions = - adapter.convert(order); - - ASSERT_EQ( - missions.size(), - 1u); - - EXPECT_EQ( - missions[0]->nodes.size(), - 4u); - - EXPECT_EQ( - missions[0]->edges.size(), - 3u); -} - -TEST(VDA5050Adapter, SplitAtNodeAction) -{ - VDA5050Adapter adapter; - - auto order = - MakeOrder(5); - - order.nodes[2].actions.push_back( - MakeAction("dock")); - - auto missions = - adapter.convert(order); - - ASSERT_EQ( - missions.size(), - 2u); - - EXPECT_EQ( - missions[0]->nodes.size(), - 3u); - - EXPECT_EQ( - missions[1]->nodes.size(), - 3u); -} - -TEST(VDA5050Adapter, CollectEdgeAction) -{ - VDA5050Adapter adapter; - - auto order = - MakeOrder(2); - - order.nodes[1].actions.push_back( - MakeAction("node")); - - order.edges[0].actions.push_back( - MakeAction("edge")); - - auto missions = - adapter.convert(order); - - ASSERT_EQ( - missions.size(), - 1u); - - ASSERT_EQ( - missions[0]->actions.size(), - 2u); - - EXPECT_LT( - missions[0]->actions[0].sequenceId, - missions[0]->actions[1].sequenceId); -} - -//-------------------------------------------------------------- -// MissionManager -//-------------------------------------------------------------- - -TEST(MissionManager, SubmitChangesState) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(1, 2))); - - EXPECT_EQ( - mgr.state(), - MissionState::QUEUED); - - EXPECT_TRUE( - mgr.hasMission()); -} - -TEST(MissionManager, NextMission) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(1, 2))); - - auto m = - mgr.nextMission(); - - ASSERT_NE( - m, - nullptr); - - EXPECT_EQ( - mgr.state(), - MissionState::RUNNING); -} - -TEST(MissionManager, NavigationDone) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(0, 0))); - - mgr.nextMission(); - - mgr.onNavigationDone(); - - EXPECT_EQ( - mgr.state(), - MissionState::IDLE); - - EXPECT_FALSE( - mgr.hasMission()); -} - -TEST(MissionManager, NavigationFailed) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(0, 0))); - - mgr.nextMission(); - - mgr.onNavigationFailed(); - - EXPECT_EQ( - mgr.state(), - MissionState::FAILED); -} - -TEST(MissionManager, PauseResume) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(1, 1))); - - mgr.pause(); - - EXPECT_EQ( - mgr.state(), - MissionState::PAUSED); - - mgr.resume(); - - EXPECT_EQ( - mgr.state(), - MissionState::QUEUED); -} - -TEST(MissionManager, Cancel) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(1, 1))); - - mgr.cancel(); - - EXPECT_EQ( - mgr.state(), - MissionState::CANCELLED); - - EXPECT_FALSE( - mgr.hasMission()); -} - -TEST(MissionManager, Emergency) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(2, 2))); - - mgr.emergency(); - - EXPECT_EQ( - mgr.state(), - MissionState::EMERGENCY); -} - -TEST(MissionManager, ClearEmergency) -{ - MissionManager mgr; - - mgr.emergency(); - - mgr.clearEmergency(); - - EXPECT_EQ( - mgr.state(), - MissionState::CLEAR_EMERGENCY); -} - -//-------------------------------------------------------------- -// EventProcessor -//-------------------------------------------------------------- - -TEST(EventProcessor, GoalEvent) -{ - MissionManager mgr; - - EventProcessor proc(mgr); - - proc.start(); - - proc.goalEvent( - MakeGoal(5, 6)); - - std::this_thread::sleep_for( - std::chrono::milliseconds(100)); - - EXPECT_EQ( - mgr.state(), - MissionState::QUEUED); - - proc.stop(); -} - -TEST(EventProcessor, EmergencyHasHighestPriority) -{ - MissionManager mgr; - - EventProcessor proc(mgr); - - proc.start(); - - proc.goalEvent( - MakeGoal(1, 1)); - - proc.emergencyEvent(); - - std::this_thread::sleep_for( - std::chrono::milliseconds(100)); - - EXPECT_EQ( - mgr.state(), - MissionState::EMERGENCY); - - proc.stop(); -} - -//-------------------------------------------------------------- -// MissionExecutor -//-------------------------------------------------------------- - -TEST(MissionExecutor, CallbackInvokedOnce) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(10, 10))); - - MissionExecutor exec(mgr); - - std::atomic count{0}; - - exec.setMissionCallback( - [&](const std::shared_ptr &) - { - count++; - }); - - exec.start(); - - std::this_thread::sleep_for( - std::chrono::milliseconds(200)); - - exec.stop(); - - EXPECT_EQ( - count.load(), - 1); -} - -TEST(MissionExecutor, NewMissionAfterDone) -{ - MissionManager mgr; - - GoalAdapter adapter; - - mgr.submit( - adapter.convert( - MakeGoal(1, 1))); - - MissionExecutor exec(mgr); - - std::atomic count{0}; - - exec.setMissionCallback( - [&](const std::shared_ptr &) - { - count++; - }); - - exec.start(); - - std::this_thread::sleep_for( - std::chrono::milliseconds(100)); - - mgr.onNavigationDone(); - - mgr.submit( - adapter.convert( - MakeGoal(2, 2))); - - std::this_thread::sleep_for( - std::chrono::milliseconds(150)); - - exec.stop(); - - EXPECT_EQ( - count.load(), - 2); -} - -//-------------------------------------------------------------- - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest( - &argc, - argv); - - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/test/mission_adapters_test.cpp b/test/mission_adapters_test.cpp new file mode 100644 index 0000000..f864927 --- /dev/null +++ b/test/mission_adapters_test.cpp @@ -0,0 +1,360 @@ +#include + +#include +#include +#include + +#include + +namespace +{ +using namespace mission_adapters; + +robot_geometry_msgs::PoseStamped makeGoal(double x, double y) +{ + robot_geometry_msgs::PoseStamped goal; + goal.pose.position.x = x; + goal.pose.position.y = y; + return goal; +} + +robot_protocol_msgs::Action makeAction(const std::string& id) +{ + robot_protocol_msgs::Action action; + action.actionId = id; + action.actionType = "TEST"; + return action; +} + +robot_protocol_msgs::Node makeNode(int sequence_id, bool add_action = false) +{ + robot_protocol_msgs::Node node; + node.sequenceId = sequence_id; + node.nodeId = "node_" + std::to_string(sequence_id); + node.nodePosition.x = sequence_id; + node.nodePosition.y = sequence_id; + + if (add_action) + node.actions.push_back(makeAction("node_action_" + std::to_string(sequence_id))); + + return node; +} + +robot_protocol_msgs::Edge makeEdge(int sequence_id, bool add_action = false) +{ + robot_protocol_msgs::Edge edge; + edge.sequenceId = sequence_id; + edge.edgeId = "edge_" + std::to_string(sequence_id); + + if (add_action) + edge.actions.push_back(makeAction("edge_action_" + std::to_string(sequence_id))); + + return edge; +} + +robot_protocol_msgs::Order makeOrder(int node_count) +{ + robot_protocol_msgs::Order order; + + for (int i = 0; i < node_count; ++i) + order.nodes.push_back(makeNode(i)); + + for (int i = 0; i < node_count - 1; ++i) + order.edges.push_back(makeEdge(i)); + + return order; +} + +bool waitForState(MissionManager& manager, MissionState expected, std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) + { + if (manager.state() == expected) + return true; + + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + + return manager.state() == expected; +} + +class MissionAdaptersTest : public ::testing::Test +{ +protected: + GoalAdapter goal_adapter; + VDA5050Adapter order_adapter; +}; + +TEST(EventBusTest, PopsHighestPriorityFirst) +{ + EventBus bus; + + Event pause; + pause.type = EventType::PAUSE; + pause.priority = PRIORITY_PAUSE; + bus.push(pause); + + Event cancel; + cancel.type = EventType::CANCEL; + cancel.priority = PRIORITY_CANCEL; + bus.push(cancel); + + Event emergency; + emergency.type = EventType::EMERGENCY; + emergency.priority = PRIORITY_EMERGENCY; + bus.push(emergency); + + Event event; + ASSERT_TRUE(bus.pop(event)); + EXPECT_EQ(event.type, EventType::EMERGENCY); + + ASSERT_TRUE(bus.pop(event)); + EXPECT_EQ(event.type, EventType::CANCEL); + + ASSERT_TRUE(bus.pop(event)); + EXPECT_EQ(event.type, EventType::PAUSE); +} + +TEST(EventBusTest, PreservesFifoOrderForSamePriority) +{ + EventBus bus; + + Event emergency; + emergency.type = EventType::EMERGENCY; + emergency.priority = PRIORITY_EMERGENCY; + bus.push(emergency); + + Event clear_emergency; + clear_emergency.type = EventType::CLEAR_EMERGENCY; + clear_emergency.priority = PRIORITY_EMERGENCY; + bus.push(clear_emergency); + + Event event; + ASSERT_TRUE(bus.pop(event)); + EXPECT_EQ(event.type, EventType::EMERGENCY); + + ASSERT_TRUE(bus.pop(event)); + EXPECT_EQ(event.type, EventType::CLEAR_EMERGENCY); +} + +TEST(EventBusTest, StopUnblocksPop) +{ + EventBus bus; + Event event; + + std::thread stopper([&bus] { bus.stop(); }); + EXPECT_FALSE(bus.pop(event)); + stopper.join(); +} + +TEST(EventBusTest, ResetDropsPendingEvents) +{ + EventBus bus; + + Event pause; + pause.type = EventType::PAUSE; + pause.priority = PRIORITY_PAUSE; + bus.push(pause); + + bus.reset(); + EXPECT_TRUE(bus.empty()); +} + +TEST_F(MissionAdaptersTest, GoalAdapterCreatesSingleMission) +{ + const auto missions = goal_adapter.convert(makeGoal(5.5, 9.1)); + + ASSERT_EQ(missions.size(), 1u); + EXPECT_EQ(missions.front()->type, MissionType::SIMPLE_GOAL); + EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 5.5); + EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 9.1); +} + +TEST_F(MissionAdaptersTest, EmptyOrderCreatesNoMissions) +{ + robot_protocol_msgs::Order order; + EXPECT_TRUE(order_adapter.convert(order).empty()); +} + +TEST_F(MissionAdaptersTest, OrderWithoutActionsCreatesOneTailMission) +{ + const auto missions = order_adapter.convert(makeOrder(4)); + + ASSERT_EQ(missions.size(), 1u); + EXPECT_EQ(missions.front()->nodes.size(), 4u); + EXPECT_EQ(missions.front()->edges.size(), 3u); +} + +TEST_F(MissionAdaptersTest, InvalidOrderWithMissingEdgesCreatesNoMissions) +{ + auto order = makeOrder(4); + order.edges.pop_back(); + + EXPECT_TRUE(order_adapter.convert(order).empty()); +} + +TEST_F(MissionAdaptersTest, OrderSplitsAtNodeAction) +{ + auto order = makeOrder(5); + order.nodes[2].actions.push_back(makeAction("dock")); + + const auto missions = order_adapter.convert(order); + + ASSERT_EQ(missions.size(), 2u); + EXPECT_EQ(missions[0]->nodes.size(), 3u); + EXPECT_EQ(missions[1]->nodes.size(), 3u); +} + +TEST_F(MissionAdaptersTest, OrderCollectsAndSortsActions) +{ + auto order = makeOrder(2); + order.edges[0].actions.push_back(makeAction("edge")); + order.nodes[1].actions.push_back(makeAction("node")); + + const auto missions = order_adapter.convert(order); + + ASSERT_EQ(missions.size(), 1u); + ASSERT_EQ(missions.front()->actions.size(), 2u); + EXPECT_EQ(missions.front()->actions[0].type, ActionType::EDGE_ACTION); + EXPECT_EQ(missions.front()->actions[1].type, ActionType::NODE_ACTION); +} + +TEST_F(MissionAdaptersTest, ManagerRunsMissionLifecycle) +{ + MissionManager manager; + + manager.submit(goal_adapter.convert(makeGoal(1.0, 2.0))); + EXPECT_EQ(manager.state(), MissionState::QUEUED); + EXPECT_TRUE(manager.hasMission()); + + auto mission = manager.nextMission(); + ASSERT_NE(mission, nullptr); + EXPECT_EQ(manager.state(), MissionState::RUNNING); + + manager.onNavigationDone(); + EXPECT_EQ(manager.state(), MissionState::IDLE); + EXPECT_FALSE(manager.hasMission()); +} + +TEST_F(MissionAdaptersTest, ManagerHandlesPauseResumeCancelAndFailure) +{ + MissionManager manager; + + manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0))); + manager.pause(); + EXPECT_EQ(manager.state(), MissionState::PAUSED); + + manager.resume(); + EXPECT_EQ(manager.state(), MissionState::QUEUED); + + manager.cancel(); + EXPECT_EQ(manager.state(), MissionState::CANCELLED); + EXPECT_FALSE(manager.hasMission()); + + manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0))); + manager.nextMission(); + manager.onNavigationFailed(); + EXPECT_EQ(manager.state(), MissionState::FAILED); + EXPECT_FALSE(manager.hasMission()); +} + +TEST_F(MissionAdaptersTest, NavigationResultIsIgnoredOutsideRunningState) +{ + MissionManager manager; + + manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0))); + manager.emergency(); + manager.onNavigationDone(); + EXPECT_EQ(manager.state(), MissionState::EMERGENCY); + + manager.onNavigationFailed(); + EXPECT_EQ(manager.state(), MissionState::EMERGENCY); + + manager.clearEmergency(); + manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0))); + manager.cancel(); + manager.onNavigationDone(); + EXPECT_EQ(manager.state(), MissionState::CANCELLED); +} + +TEST_F(MissionAdaptersTest, EmergencyClearsAndBlocksNewMissionsUntilCleared) +{ + MissionManager manager; + + manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0))); + manager.emergency(); + EXPECT_EQ(manager.state(), MissionState::EMERGENCY); + EXPECT_FALSE(manager.hasMission()); + + manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0))); + EXPECT_EQ(manager.state(), MissionState::EMERGENCY); + EXPECT_FALSE(manager.hasMission()); + + manager.clearEmergency(); + manager.submit(goal_adapter.convert(makeGoal(3.0, 3.0))); + EXPECT_EQ(manager.state(), MissionState::QUEUED); + EXPECT_TRUE(manager.hasMission()); +} + +TEST_F(MissionAdaptersTest, EventProcessorProcessesGoalAndEmergency) +{ + MissionManager manager; + EventProcessor processor(manager); + + processor.start(); + processor.goalEvent(makeGoal(5.0, 6.0)); + EXPECT_TRUE(waitForState(manager, MissionState::QUEUED, std::chrono::milliseconds(250))); + + processor.emergencyEvent(); + EXPECT_TRUE(waitForState(manager, MissionState::EMERGENCY, std::chrono::milliseconds(250))); + + processor.goalEvent(makeGoal(7.0, 8.0)); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_EQ(manager.state(), MissionState::EMERGENCY); + + processor.clearEmergencyEvent(); + EXPECT_TRUE(waitForState(manager, MissionState::CLEAR_EMERGENCY, std::chrono::milliseconds(250))); + + processor.stop(); +} + +TEST_F(MissionAdaptersTest, MissionExecutorDispatchesEachMissionOnce) +{ + MissionManager manager; + MissionExecutor executor(manager); + std::atomic callback_count{0}; + + executor.setMissionCallback( + [&callback_count](const std::shared_ptr& mission) + { + ASSERT_NE(mission, nullptr); + ++callback_count; + }); + + manager.submit(goal_adapter.convert(makeGoal(10.0, 10.0))); + executor.start(); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(250); + while (callback_count.load() < 1 && std::chrono::steady_clock::now() < deadline) + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + manager.onNavigationDone(); + manager.submit(goal_adapter.convert(makeGoal(11.0, 11.0))); + + const auto second_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(250); + while (callback_count.load() < 2 && std::chrono::steady_clock::now() < second_deadline) + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + executor.stop(); + + EXPECT_EQ(callback_count.load(), 2); +} + +} // namespace + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}