add file test

src/robot_control_test.cpp

@@ -1,6 +1,8 @@
 #include <mission_adapters/mission_adapters.h>
 #include <move_base_core/navigation.h>
 
+#include <utility>
+
 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>& 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;
-}