diff --git a/CMakeLists.txt b/CMakeLists.txt index 7b893e1..fc8dce2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -262,3 +262,58 @@ else() endif() endif() + +# ======================================================== +# Unit Test +# ======================================================== +if(CATKIN_ENABLE_TESTING) + + find_package(catkin REQUIRED COMPONENTS + robot_costmap_2d + robot_nav_core + robot_nav_core2 + robot_nav_msgs + robot_std_msgs + robot_geometry_msgs + robot_cpp + robot_tf3_geometry_msgs + robot_visualization_msgs + robot_nav_2d_utils + data_convert + ) + + message(STATUS "CATKIN_ENABLE_TESTING=${CATKIN_ENABLE_TESTING}") + + if(COMMAND catkin_add_gtest) + message(STATUS "catkin_add_gtest exists") + else() + 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} + ) + + if(TARGET test_mission_adapters) + target_link_libraries(test_mission_adapters + mission_adapters + ${catkin_LIBRARIES} + ) + + target_include_directories(test_mission_adapters PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${catkin_INCLUDE_DIRS} + ) + endif() + +endif() diff --git a/include/mission_adapters/mission_adapters.h b/include/mission_adapters/mission_adapters.h index 0dc71f8..4b0f057 100644 --- a/include/mission_adapters/mission_adapters.h +++ b/include/mission_adapters/mission_adapters.h @@ -33,12 +33,12 @@ namespace mission_adapters QUEUED, RUNNING, PAUSED, - WAITING_ACTION, RECOVERY, COMPLETED, FAILED, CANCELLED, - EMERGENCY + EMERGENCY, + CLEAR_EMERGENCY }; enum class EventType @@ -49,7 +49,8 @@ namespace mission_adapters PAUSE, RESUME, CANCEL, - EMERGENCY + EMERGENCY, + CLEAR_EMERGENCY }; enum class MissionType @@ -110,6 +111,7 @@ namespace mission_adapters public: void push(const Event& event); + void push(Event&& event); bool pop(Event& event); void stop(); void reset(); @@ -152,6 +154,7 @@ namespace mission_adapters void pause(); void resume(); void emergency(); + void clearEmergency(); MissionState state() const; @@ -182,6 +185,7 @@ namespace mission_adapters void navDoneEvent(); void navFailedEvent(); void emergencyEvent(); + void clearEmergencyEvent(); private: void spin(); diff --git a/package.xml b/package.xml index 0a2a555..16f2806 100644 --- a/package.xml +++ b/package.xml @@ -47,4 +47,5 @@ robot_visualization_msgs robot_nav_2d_utils data_convert + gtest \ No newline at end of file diff --git a/src/mission_adapters.cpp b/src/mission_adapters.cpp index 4edfd59..3a20c46 100644 --- a/src/mission_adapters.cpp +++ b/src/mission_adapters.cpp @@ -1,3 +1,4 @@ +//file mission_adapters.cpp #include #include @@ -14,6 +15,13 @@ namespace mission_adapters cv_.notify_one(); } + void EventBus::push(Event&& event) + { + std::lock_guard lock(mutex_); + queue_.push(std::move(event)); + cv_.notify_one(); + } + bool EventBus::pop(Event& event) { std::unique_lock lock(mutex_); @@ -66,6 +74,8 @@ namespace mission_adapters std::vector> missions; std::vector action_node_indices; + if(order.nodes.empty()) return missions; + for (size_t i = 0; i < order.nodes.size(); ++i) { if (!order.nodes[i].actions.empty()) @@ -126,9 +136,7 @@ namespace mission_adapters for (const auto& action : edge.actions) { Action ma; - // FIX #6: Use action.sequenceId, not edge.sequenceId, - // so the sort below produces the correct execution order. - ma.sequenceId = action.sequenceId; + ma.sequenceId = edge.sequenceId; ma.type = ActionType::EDGE_ACTION; ma.action = action; mission->actions.push_back(std::move(ma)); @@ -139,8 +147,7 @@ namespace mission_adapters for (const auto& action : node.actions) { Action ma; - // FIX #6: Same fix for node actions. - ma.sequenceId = action.sequenceId; + ma.sequenceId = node.sequenceId; ma.type = ActionType::NODE_ACTION; ma.action = action; mission->actions.push_back(std::move(ma)); @@ -162,6 +169,12 @@ namespace mission_adapters void MissionManager::submit(const std::vector>& missions) { std::lock_guard lock(mutex_); + if (!mission_queue_.empty()) + { + mission_queue_ = {}; + current_mission_.reset(); + } + for (const auto& mission : missions) mission_queue_.push(mission); @@ -176,6 +189,7 @@ namespace mission_adapters case MissionState::FAILED: case MissionState::CANCELLED: case MissionState::EMERGENCY: + case MissionState::CLEAR_EMERGENCY: state_ = MissionState::QUEUED; break; default: @@ -188,10 +202,12 @@ namespace mission_adapters { std::lock_guard lock(mutex_); - if (state_ == MissionState::PAUSED || + if (state_ == MissionState::IDLE || + state_ == MissionState::PAUSED || state_ == MissionState::FAILED || state_ == MissionState::CANCELLED || - state_ == MissionState::EMERGENCY) + state_ == MissionState::EMERGENCY || + state_ == MissionState::CLEAR_EMERGENCY) { return nullptr; } @@ -231,6 +247,7 @@ namespace mission_adapters std::lock_guard lock(mutex_); current_mission_.reset(); while (!mission_queue_.empty()) mission_queue_.pop(); + if(state_ == MissionState::EMERGENCY) return; state_ = MissionState::CANCELLED; } @@ -242,10 +259,19 @@ namespace mission_adapters state_ = MissionState::EMERGENCY; } + void MissionManager::clearEmergency() + { + std::lock_guard lock(mutex_); + if(state_ == MissionState::EMERGENCY) + state_ = MissionState::CLEAR_EMERGENCY; + } + void MissionManager::pause() { std::lock_guard lock(mutex_); - if (state_ == MissionState::RUNNING) + if (state_ == MissionState::IDLE || + state_ == MissionState::RUNNING || + state_ == MissionState::QUEUED) state_ = MissionState::PAUSED; } @@ -256,7 +282,7 @@ namespace mission_adapters if (current_mission_) state_ = MissionState::RUNNING; - else if (!mission_queue_.empty()) + else if (!current_mission_ && !mission_queue_.empty()) state_ = MissionState::QUEUED; else state_ = MissionState::IDLE; @@ -321,6 +347,7 @@ namespace mission_adapters case EventType::RESUME: mission_manager_.resume(); break; case EventType::CANCEL: mission_manager_.cancel(); break; case EventType::EMERGENCY: mission_manager_.emergency(); break; + case EventType::CLEAR_EMERGENCY: mission_manager_.clearEmergency(); break; default: robot::log_error("Unknown event type"); break; @@ -376,6 +403,11 @@ namespace mission_adapters event_bus_.push({EventType::EMERGENCY, PRIORITY_EMERGENCY, {}}); } + void EventProcessor::clearEmergencyEvent() + { + event_bus_.push({EventType::CLEAR_EMERGENCY, PRIORITY_EMERGENCY, {}}); + } + // ───────────────────────────────────────────────────────────────────────── // MissionExecutor // ───────────────────────────────────────────────────────────────────────── diff --git a/src/robot_control_test.cpp b/src/robot_control_test.cpp index 27c9b54..bd80e67 100644 --- a/src/robot_control_test.cpp +++ b/src/robot_control_test.cpp @@ -23,7 +23,7 @@ class RobotControlTest } public: - RobotControlTest() = default; + RobotControlTest(); ~RobotControlTest(); void run(); @@ -32,6 +32,9 @@ private: void executeMission(const Mission& mission); }; +RobotControlTest::RobotControlTest() +{} + RobotControlTest::~RobotControlTest() { event_processor_.stop(); @@ -49,27 +52,22 @@ void RobotControlTest::run() executeMission(*mission); }); - event_processor_.start(); - mission_executor_.start(); + while (true) { auto feedback = move_base_ptr_->getFeedback(); auto nav_state = feedback->navigation_state; - // FIX #8: navDoneEvent fires only when navigation AND actions are both done. - if (nav_state != prev_nav_state_) + if (nav_state != prev_nav_done_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone()) { - if (nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone()) - { - event_processor_.navDoneEvent(); - } - else if (nav_state == robot::move_base_core::State::ABORTED) - { - event_processor_.navFailedEvent(); - } - prev_nav_state_ = nav_state; + event_processor_.navDoneEvent(); } + else if (nav_state == robot::move_base_core::State::ABORTED) + { + event_processor_.navFailedEvent(); + } + prev_nav_state_ = nav_state; // Example: receive an order (replace condition with your real source) if (/* new order available */ false) diff --git a/src/test_mission_adapters b/src/test_mission_adapters new file mode 100755 index 0000000..80f4a89 Binary files /dev/null and b/src/test_mission_adapters differ diff --git a/src/test_mission_adapters.cpp b/src/test_mission_adapters.cpp new file mode 100755 index 0000000..0de5e50 --- /dev/null +++ b/src/test_mission_adapters.cpp @@ -0,0 +1,578 @@ +#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