From 5c2f552ffac7064ed551aad784e79570a5109f7b Mon Sep 17 00:00:00 2001
From: Chiyu Chen <chiyu1468@hotmail.com>
Date: Mon, 1 Jun 2026 16:38:27 +0800
Subject: [PATCH] =?UTF-8?q?(hot=5Ffix)=201.=20=E6=8A=8A=E6=B8=AC=E8=A9=A6?=
 =?UTF-8?q?=E7=94=A8=E5=8F=83=E6=95=B8=E9=97=9C=E9=96=89=202.=20=E8=87=A8?=
 =?UTF-8?q?=E6=99=82=E5=8A=A0=E5=85=A5=20node=20=E5=A4=B1=E6=95=88?=
 =?UTF-8?q?=E5=BE=8C=20=E8=87=AA=E5=8B=95=E9=87=8D=E5=95=9F=E5=8A=9F?=
 =?UTF-8?q?=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../fc_network_adapter/mainOrchestrator.py        | 15 +++++++++++++++
 .../fc_network_adapter/mavlinkROS2Nodes.py        |  2 +-
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
index f3764c6..c183d32 100644
--- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
@@ -1218,6 +1218,8 @@ class Orchestrator:
 
     def mainLoop(self):
         logger.info("Main orchestrator started <-")
+        self.ros2_retry_time = None
+        self.ros2_manual_stop = False
         try:
             # while not self.stop_evt.is_set():
             while self.panel_thread.is_alive():
@@ -1249,6 +1251,17 @@ class Orchestrator:
                 else:
                     self.panelState.ros2_manager_state = 'Stopped'
 
+                    # 這邊做一個簡易的重啟 掛掉要等待一秒再重啟 避免過度頻繁 # 算是高技術債的方式構建 但是未來有需要在還吧 (煙
+                    if not self.ros2_manual_stop:
+                        # logger.info(f"mark A {self.ros2_retry_time}")
+                        if self.ros2_retry_time:
+                            if (time.time() - self.ros2_retry_time) > 1:
+                                self.fc_ros_manager.start()
+                                self.fc_ros_manager.schedule_restart_node("command_service")
+                                self.ros2_retry_time = None
+                        else:
+                            self.ros2_retry_time = time.time()
+
                 # B. 更新載具列表（從 vehicle_registry 獲取）
                 self._update_vehicles_list()
 
@@ -1337,8 +1350,10 @@ class Orchestrator:
                         self.plumber.shutdown()
                     elif cmd == "STOP_ROS_MANAGER":
                         self.fc_ros_manager.stop()
+                        self.ros2_manual_stop = True
                     elif cmd == "START_ROS_MANAGER":
                         self.fc_ros_manager.start()
+                        self.ros2_manual_stop = False
 
                 except queue.Empty:
                     pass
diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
index 4d61832..7d72a50 100644
--- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
+++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
@@ -758,7 +758,7 @@ class MavlinkCommandService(Node):
         # 6) 處理回應封包
         if not fail_skip:
             ack_msg = _pending.result_mav_msg
-            response.success = ServiceAckResult.FCSYS_MAVLINK_BREAK # dev 故意觸發錯誤用的
+            # response.success = ServiceAckResult.FCSYS_MAVLINK_BREAK # dev 故意觸發錯誤用的
             response.success = (ack_msg.result == 0) # mavutil.mavlink.MAV_RESULT_ACCEPTED
             response.message = "" # 沒有消息就是好消息
             response.ack_result = ack_msg.result