diff --git a/README.md b/README.md index 717c75d..dd27db2 100644 --- a/README.md +++ b/README.md @@ -2,9 +2,15 @@ === ## 功能簡介 -1. mavlink 多對多支援平台 -2. 不允許進到 ros 系統有相同 sysid -3. 假設一台載具上所有 component 共用同一 socket +1. mavlink 多對多串流與交換 +2. 支援 xbee 與 telemetry +3. 支援 udp 管理 +4. ROS2 介面監控載具 + +=== +使用限制 +1. 不允許進到 ros 系統有相同 sysid +2. 假設一台載具上所有 component 共用同一 socket === ## 運行環境 diff --git a/src/fc_interfaces/CMakeLists.txt b/src/fc_interfaces/CMakeLists.txt index 36b3500..1fe91d1 100644 --- a/src/fc_interfaces/CMakeLists.txt +++ b/src/fc_interfaces/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(builtin_interfaces REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/AttitudeRaw.msg" "msg/GnssRaw.msg" + "msg/ServiceAckResult.msg" "srv/MavPing.srv" "srv/MavCommandLong.srv" "srv/MavPositionTargetGlobalInt.srv" diff --git a/src/fc_interfaces/msg/ServiceAckResult.msg b/src/fc_interfaces/msg/ServiceAckResult.msg new file mode 100644 index 0000000..f414879 --- /dev/null +++ b/src/fc_interfaces/msg/ServiceAckResult.msg @@ -0,0 +1,43 @@ +# ServiceAckResult.msg +# ACK code definition for command-like service responses. +# 這邊只是定義常數的檔案 +# +# Design: +# - 0~10 : MAVLink MAV_RESULT compatible range (autopilot/firmware ACK) 大多不會用到 直接吃飛控回傳值 +# - 50~89 : System-level ACK codes (local bridge/orchestrator status) +# - 90~99 : Unknown/reserved fallback +# +# Rule: +# - If FC ACK is received, return MAV_RESULT_* code. +# - If FC ACK is not reachable / local pipeline failed, return SYS_* code. + +# ---- MAVLink MAV_RESULT (standard) ---- 詳細請自行去網路找 mavlink MAV_RESULT +uint8 MAV_RESULT_ACCEPTED=0 # 封包正常接受並執行 +uint8 MAV_RESULT_TEMPORARILY_REJECTED=1 # 封包正常接受 但是飛控很忙 不執行 需要等待後重試 +uint8 MAV_RESULT_DENIED=2 # 封包不正常 參數可能給錯了 +uint8 MAV_RESULT_UNSUPPORTED=3 # 封包不正常 根本認不出是啥 +uint8 MAV_RESULT_FAILED=4 # 封包正常接受 但是載具的狀態異常 不執行 +uint8 MAV_RESULT_IN_PROGRESS=5 +uint8 MAV_RESULT_CANCELLED=6 +uint8 MAV_RESULT_COMMAND_LONG_ONLY=7 +uint8 MAV_RESULT_COMMAND_INT_ONLY=8 +uint8 MAV_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME=9 +uint8 MAV_RESULT_NOT_IN_CONTROL=10 + +# ---- System-level result codes (custom) ---- +# 50 : 有接到預期的封包 但是訊息建議再確認 +# 51 : some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair +# +# 52 : mavlink communication timeout 可能是通訊丟包了 +# 53 : there already has a command being execution for this sysid, try later +# 54 : this sysid is not found + +uint8 FCSYS_GET_EXPECTED_FEEDBACK=50 +uint8 FCSYS_NO_EXPECTED_FEEDBACK=51 +uint8 FCSYS_MAVLINK_TIMEOUT=52 +uint8 FCSYS_BUSY=53 +uint8 FCSYS_DEVICE_NOT_FOUND=54 + + +# ---- Fallback ---- +uint8 FCSYS_UNKNOWN=90 \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/ackEnum.py b/src/fc_network_adapter/fc_network_adapter/ackEnum.py deleted file mode 100644 index aefd253..0000000 --- a/src/fc_network_adapter/fc_network_adapter/ackEnum.py +++ /dev/null @@ -1,31 +0,0 @@ - -''' -單獨出來定義回傳碼的意義 -若跟飛控韌體有正常的通訊 (無論是否被拒絕) 則以 mavlink 定義的結果回傳 -若跟飛控韌體無法通訊 才使用這邊的錯誤代碼 -所以要避開重疊的號碼 - -這邊備註 Mavlink 的 MAV_RESULT 參數意義 -0 : 封包正常接受並執行 -1 : 封包正常接受 但是正在忙不執行 需要等待後重試 -4 : 封包正常接受 但是載具的狀態異常 不執行 - -2 : 封包不正常 參數可能給錯了 -3 : 封包不正常 根本認不出是啥 - -''' - - -from enum import IntEnum - -class serviceAckResult(IntEnum): - MAVLINK_TIMEOUT = 51 # mavlink communication timeout - - MAVLINK_SEND_BUT_NO_EXP_STEAM = 30 # some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair - - MAVLINK_BUSY = 53 # there already has a command being execution for this sysid, try later - MAVLINK_DEV_NOTFOUND = 54 # this sysid not fount - - UNKNOWN = 90 # i did not capture this error, try read the logs - - diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md index b234bb6..2f7b213 100644 --- a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md +++ b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md @@ -37,6 +37,7 @@ - *self.bridge* 存放 mavlink_bridge 實例 - *self.plumber* 存放 serial_manager 實例 - *self.vehicle_registry* 存放 vehicle_registry 實例 +- *self.fc_ros_manager* 存放 fc_ros_manager 實例 - *self.panel_thread* 面板的執行緒 - *self.panelState* 暫存面板與調配者互動的資料流動區 @@ -240,8 +241,14 @@ 8. 基礎載具流量觀測 9. 載具狀態收集與彙整 10. a. ros2 topic 應用開發介面 (基礎框架) + - GNSS (alt lan fix_type epv eph) + - attitude position_ned (pitch yaw row local_position) + - battery b. ros2 service 應用開發介面 (基礎框架) -11. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具 + c. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具 +11. ros2 topic service rtcm Node 重啟功能 +12. 可以定義 ROS_DOMAIN_ID + ### 待開發功能 5-1. 建立 serial 連線 並可以對接收器下達AT指令 diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index ddf2933..f3764c6 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort from .utils.acquirePort import find_available_port logger = setup_logger(os.path.basename(__file__)) -PROJECT_VER = "v0.70" +PROJECT_VER = "v1.00" class PanelState: def __init__(self): @@ -177,11 +177,18 @@ class ControlPanel: MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"), MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"), ]), - MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"), + MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "SHOW_VEHICLES"), MenuNode("Engineer Mode", "工程模式", children=[ - MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), + MenuNode("Stop Manager", "停止 Mavlink_Object 管理", "STOP_MANAGER"), MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), - MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), + MenuNode("Stop Serial M.", "停止 Serial 端口", "STOP_SERIAL_MANAGER"), + MenuNode("Edit ROS Nodes", "管理 ROS NODE", children=[ + MenuNode("Reboot Topics", "重建 ROS Topics", "REBOOT_ROS_TOPICS"), + MenuNode("Reboot Services", "重建 ROS Services", "REBOOT_ROS_SERVICES"), + MenuNode("Reboot RTCM", "重建 RTK RTCM", "REBOOT_ROS_RTCM"), + MenuNode("Stop ROS Manager", "停止 ROS 功能", "STOP_ROS_MANAGER"), + MenuNode("Start ROS Manager", "啟動 ROS 功能", "START_ROS_MANAGER"), + ]), ]), MenuNode("Shutdown", "關閉整個系統", children=[ MenuNode("Return", "繼續運行", "BACK"), @@ -384,6 +391,7 @@ class ControlPanel: state.intoTERMINATION() pre_panel_shutdown() + # related with -> UDP mavlink Object elif selected.action == "TEXT_UDP_IP": result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ") if result is not None: @@ -412,6 +420,8 @@ class ControlPanel: # menu_stack.pop() # idx_stack.pop() + + # related with -> Serial Connection elif selected.action == "TEXT_BAUD_SERIAL": result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ") if result is not None: @@ -477,12 +487,7 @@ class ControlPanel: menu_stack.pop() idx_stack.pop() - elif selected.action == "LIST_MAV_OBJECT": - # 動態生成 mavlink_object 列表選單 - created_list_menu = self.create_object_list_menu(state, page=0) - menu_stack.append(created_list_menu) - idx_stack.append(0) - + # related with -> 列表選單 elif selected.action in ("PREV_PAGE", "NEXT_PAGE"): if hasattr(selected, 'page'): current_list_menu = menu_stack[-1] @@ -507,6 +512,13 @@ class ControlPanel: menu_stack.append(created_list_menu) idx_stack.append(0) + # related with -> mavlink object list + elif selected.action == "LIST_MAV_OBJECT": + # 動態生成 mavlink_object 列表選單 + created_list_menu = self.create_object_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + elif selected.action == "INSPECT_MAV_OBJECT": # 顯示物件詳細資訊 if hasattr(selected, 'socket_id'): @@ -551,6 +563,7 @@ class ControlPanel: if target_id is not None: cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + # related with -> Engineer Mode elif selected.action == "STOP_MANAGER": if state.panel_status != "Engineer": state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) @@ -568,8 +581,40 @@ class ControlPanel: state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) continue # 只有在工程模式下才能操作 cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + + elif selected.action == "STOP_ROS_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("STOP_ROS_MANAGER") - elif selected.action == "INSPECT_VEHICLES": + elif selected.action == "START_ROS_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("START_ROS_MANAGER") + + + elif selected.action == "REBOOT_ROS_TOPICS": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put(("RESTART_ROS_NODE", "status_publisher")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS + + elif selected.action == "REBOOT_ROS_SERVICES": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put(("RESTART_ROS_NODE", "command_service")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS + + elif selected.action == "REBOOT_ROS_RTCM": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put(("RESTART_ROS_NODE", "rtcm_relay")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS + + # related with -> 進 bridge 的設備 列表 + elif selected.action == "SHOW_VEHICLES": # 進入載具檢視選單 cmd_q.put("UPDATE_VEHICLES_LIST") created_list_menu = self.create_vehicles_list_menu(state, page=0) @@ -1268,6 +1313,12 @@ class Orchestrator: elif action == "UNREGISTER_VEHICLE": sysid = cmd[1] self.vehicle_registry.unregister(sysid) + elif action == "RESTART_ROS_NODE": + node_key = cmd[1] + if not self.fc_ros_manager.schedule_restart_node(node_key): + logger.warning( + f"RESTART_ROS_NODE failed: {node_key!r}" + ) elif cmd == "CREATE_UDP_INBOUND": self.panelState.udp_info_temp["direction"] = "inbound" @@ -1277,12 +1328,17 @@ class Orchestrator: self.create_udp_object() elif cmd == "CREATE_SERIAL_PORT": self.create_serial_port_object() + elif cmd == "SHUTDOWN_BRIDGE": self.bridge.stop() elif cmd == "SHUTDOWN_MANAGER": self.manager.shutdown() elif cmd == "SHUTDOWN_SERIAL_MANAGER": self.plumber.shutdown() + elif cmd == "STOP_ROS_MANAGER": + self.fc_ros_manager.stop() + elif cmd == "START_ROS_MANAGER": + self.fc_ros_manager.start() except queue.Empty: pass @@ -1571,7 +1627,7 @@ def main(): if mo.MODULE_VER != "1.50": print("Module Version Error! : mavlinkObkect") version_check = False - if mros.MODULE_VER != "1.50": + if mros.MODULE_VER != "2.10": print("Module Version Error! : mavlinkROS2Nodes") version_check = False if mvv.MODULE_VER != "1.00": @@ -1622,4 +1678,7 @@ if __name__ == "__main__": 2025.04.13 1. 加入各項模組的版本先驗 檢驗失敗就直接離開 +2025.05.28 +1. 工程模式 新增 Node 重啟的功能 + ''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index ff8a4f2..4d61832 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -37,7 +37,7 @@ import mavros_msgs.msg # ROS2 Service imports import fc_interfaces.srv as fcsrv import fc_interfaces.msg as fcmsg -from .ackEnum import serviceAckResult +from fc_interfaces.msg import ServiceAckResult # 自定義 imports from . import mavlinkVehicleView as mvv @@ -45,7 +45,10 @@ from . import mavlinkObject as mo from .utils import setup_logger logger = setup_logger(os.path.basename(__file__)) -MODULE_VER = "1.50" +MODULE_VER = "2.10" + +FC_ROS_DOMAIN_ID = "0" +NODE_KEYS = ("status_publisher", "command_service", "rtcm_relay") # ============================================================================ # VehicleStatusPublisher Node @@ -535,7 +538,7 @@ class MavlinkCommandService(Node): def __init__(self): super().__init__('mavlink_command_service') - # 狀態標記 + # 狀態標記 self.running = True # # mavlinkObject 的引用（將由外部設置） 用不到 @@ -581,7 +584,7 @@ class MavlinkCommandService(Node): self.handle_set_position_target_global_int ) - logger.info("MavlinkCommandService initialized") + # logger.info("MavlinkCommandService initialized") def _index(self, target_sysid, target_compid): @@ -608,20 +611,19 @@ class MavlinkCommandService(Node): 藉由 event.set() 解開 service 中的 block ''' return_tuple = mo.return_packet_ring.get() - # 確認 return_packet_ring 有資料 if return_tuple == None: return socketid, timestamp, msg = return_tuple sysid = msg.get_srcSystem() _pending = self._pending_by_sysid.get(sysid) - # 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略 - if _pending == None: + if _pending is None: return if _pending.match_fn(msg): _pending.result_mav_msg = msg _pending.event.set() + return # ═══════════════════════════════════════════════════════════════════════ # Service Handler: 發送 TIMESYNC 可以作為模板 @@ -706,20 +708,20 @@ class MavlinkCommandService(Node): # 設定失效回應 response.success = False response.message = "Unknown error" - response.ack_result = serviceAckResult.UNKNOWN + response.ack_result = ServiceAckResult.FCSYS_UNKNOWN timeout_sec = request.timeout_sec # 1) 確認是否已經有相同 sysid 的其他需求正在 pending if request.target_sysid in self._pending_by_sysid: response.message = f"sysid {request.target_sysid} already has pending request" - response.ack_result = serviceAckResult.MAVLINK_BUSY + response.ack_result = ServiceAckResult.FCSYS_BUSY return response # 2) 找到 socket 標地 socketObject = self._index(request.target_sysid, request.target_compid) if socketObject is None: response.message = "This system id not found." - response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND + response.ack_result = ServiceAckResult.FCSYS_DEVICE_NOT_FOUND return response # 3) 接收封包系統 的設定 @@ -750,12 +752,13 @@ class MavlinkCommandService(Node): # 5) 等待回應封包 if not evt.wait(timeout_sec): response.message = "mavlink timeout - CommLONG" - response.ack_result = serviceAckResult.MAVLINK_TIMEOUT + response.ack_result = ServiceAckResult.FCSYS_MAVLINK_TIMEOUT fail_skip = True # 6) 處理回應封包 if not fail_skip: ack_msg = _pending.result_mav_msg + 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 @@ -779,20 +782,20 @@ class MavlinkCommandService(Node): ''' # 設定失效回應 response.message = "Unknown error" - response.ack_result = serviceAckResult.UNKNOWN + response.ack_result = ServiceAckResult.FCSYS_UNKNOWN timeout_sec = request.timeout_sec # 1) 確認是否已經有相同 sysid 的其他需求正在 pending if request.target_sysid in self._pending_by_sysid: response.message = f"sysid {request.target_sysid} already has pending request" - response.ack_result = serviceAckResult.MAVLINK_BUSY + response.ack_result = ServiceAckResult.FCSYS_BUSY return response # 2) 找到 socket 標地 socketObject = self._index(request.target_sysid, request.target_compid) if socketObject is None: response.message = "This system id not found." - response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND + response.ack_result = ServiceAckResult.FCSYS_DEVICE_NOT_FOUND return response # 3) 接收封包系統 的設定 @@ -827,7 +830,7 @@ class MavlinkCommandService(Node): # 5) 等待回應封包 if not evt.wait(timeout_sec): response.message = "mavlink timeout - SET POSITION 86" - response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM + response.ack_result = ServiceAckResult.FCSYS_NO_EXPECTED_FEEDBACK fail_skip = True # 6) 處理回應封包 @@ -835,7 +838,7 @@ class MavlinkCommandService(Node): ack_msg = _pending.result_mav_msg response.message = "" - response.ack_result = 0 + response.ack_result = ServiceAckResult.FCSYS_GET_EXPECTED_FEEDBACK response.r_time_boot_ms = ack_msg.time_boot_ms response.r_type_mask = ack_msg.type_mask response.r_lat_int = ack_msg.lat_int @@ -952,11 +955,10 @@ class MavlinkCommandService(Node): return response - def stop(self): """停止服務""" self.running = False - logger.info("MavlinkCommandService stopped") + # logger.info("MavlinkCommandService stopped") # ============================================================================ @@ -1025,7 +1027,7 @@ class RtcmRelay(Node): rtcm_qos, ) - logger.info("RtcmRelay initialized") + # logger.info("RtcmRelay initialized") # ── callback ────────────────────────────────────────────────────── @@ -1129,7 +1131,7 @@ class RtcmRelay(Node): def stop(self): """停止轉發""" self.running = False - logger.info("RtcmRelay stopped") + # logger.info("RtcmRelay stopped") # ============================================================================ @@ -1145,7 +1147,7 @@ class fc_ros_manager: stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。 shutdown 是完全關閉 ROS2 並銷毀節點實例。 - 管理三個獨立的 ROS2 Node： + 管理三個獨立的 ROS2 Node : - VehicleStatusPublisher - MavlinkCommandService - RtcmRelay @@ -1156,6 +1158,9 @@ class fc_ros_manager: def __init__(self): self.initialized = False self.running = False + self._restarting = False + self._pending_restart: Optional[str] = None + self._restart_lock = threading.Lock() # node 實例 self.status_publisher: Optional[VehicleStatusPublisher] = None @@ -1173,7 +1178,7 @@ class fc_ros_manager: return False try: - # 初始化 ROS2 + os.environ.setdefault("ROS_DOMAIN_ID", FC_ROS_DOMAIN_ID) if not rclpy.ok(): rclpy.init(args=None) @@ -1208,6 +1213,10 @@ class fc_ros_manager: try: self.running = True + + self.status_publisher.running = True + self.command_service.running = True + self.rtcm_relay.running = True self.spin_thread = threading.Thread( target=self._spin_executor, @@ -1223,24 +1232,105 @@ class fc_ros_manager: logger.error(f"Failed to start fc_ros_manager: {e}") self.running = False return False - + + # 給 orchestrator 呼叫的函數 + def schedule_restart_node(self, node_key: str) -> bool: + """由 orchestrator 排程單一 node 重啟（於 spin thread 下一輪執行）""" + if node_key not in NODE_KEYS: + logger.warning(f"schedule_restart_node: unknown key {node_key!r}") + return False + if not self.running: + logger.warning("schedule_restart_node: fc_ros_manager not running") + return False + if not self.spin_thread or not self.spin_thread.is_alive(): + logger.warning("schedule_restart_node: spin thread not alive") + return False + with self._restart_lock: + self._pending_restart = node_key + logger.info(f"schedule_restart_node: queued {node_key}") + return True + # 循環執行的地方 def _spin_executor(self): """在 thread 中運行的 executor""" try: - # logger.info("ROS2 executor spinning...") while self.running: self.executor.spin_once(timeout_sec=0.1) - self.command_service.return_router() + + pending = None + with self._restart_lock: + if self._pending_restart: + pending = self._pending_restart + self._pending_restart = None + + if pending: + self._restart_node(pending) + elif self.command_service and not self._restarting: + self.command_service.return_router() except Exception as e: logger.error(f"fc_ros_manager error in spinning executor: {e}") - + finally: + self.running = False + + def _restart_node(self, node_key: str) -> bool: + """僅由 spin thread 呼叫 : destroy 並重建單一 node""" + old_node = getattr(self, node_key, None) + if old_node is None: + logger.error(f"_restart_node: {node_key} is None") + return False + + # 重置前保留舊有或處理設定 + saved_topic_intervals = None + if node_key == "status_publisher": + saved_topic_intervals = dict(old_node.rate_controller.topic_intervals) + + # 如果是 command_service 清除所有等待的 pending 並且清除 return message + if node_key == "command_service": + old_node._pending_by_sysid.clear() + for sockObj in mo.mavlink_object.mavlinkObjects.values(): + sockObj.set_return_message_types([]) + + try: + self._restarting = True + + # 停止運作 移出執行迴圈 銷毀資源 + old_node.stop() + self.executor.remove_node(old_node) + old_node.destroy_node() + + # 建立新的 node + if node_key == "status_publisher": + new_node = VehicleStatusPublisher() + if saved_topic_intervals is not None: + new_node.rate_controller.topic_intervals = saved_topic_intervals + elif node_key == "command_service": + new_node = MavlinkCommandService() + elif node_key == "rtcm_relay": + new_node = RtcmRelay() + else: + return False + + # 加回去管理系統中 + setattr(self, node_key, new_node) + self.executor.add_node(new_node) + logger.info(f"_restart_node: {node_key} restarted") + return True + except Exception as e: + logger.error(f"_restart_node failed for {node_key}: {e}") + return False + finally: + self._restarting = False + def stop(self): """停止 ROS2 nodes""" - if not self.running: - logger.warning("fc_ros_manager not running") + # if not self.running: + # logger.warning("fc_ros_manager not running") + # return False + + if not self.spin_thread: + logger.warning("fc_ros_manager without thread to stop") return False - + try: # 標記停止 self.running = False @@ -1289,7 +1379,7 @@ class fc_ros_manager: except Exception as e: logger.error(f"Error during shutdown: {e}") - def get_status(self) -> dict: + def get_status(self): return { 'initialized': self.initialized, 'running': self.running, @@ -1298,6 +1388,12 @@ class fc_ros_manager: 'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running, } + + + + + + # ============================================================================ # 全域實例 @@ -1331,8 +1427,13 @@ ros2_manager = fc_ros_manager() 2. 實作過期丟棄(1s)、blake2s hash 去重、最短轉發間隔節流、分片(180 bytes/片) 3. 接入 fc_ros_manager 的 initialize/start/stop/shutdown 生命週期 +2026.05.22 +1. initialize() 設定 ROS_DOMAIN_ID ( FC_ROS_DOMAIN_ID ) +2. schedule_restart_node / _restart_node : 手動重啟單一 node (spin thread 內執行） +3. orchestrator cmd: ("RESTART_ROS_NODE", node_key), node_key 見 NODE_KEYS + TODO 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期 -2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題 + ''' diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt index 835e70d..df3e429 100644 --- a/src/unitdev02/unitdev02/devnote.txt +++ b/src/unitdev02/unitdev02/devnote.txt @@ -1,32 +1,25 @@ 備選需要的功能 - - serail 對於 telemetry 的支援 - serial_manager.serial_object.transport 這些變數可能不需要 不用動 - mavlink_object 的 send_message 確認一下 mavlink_bridge 的 _send_to_socket 是不是應該做成 async - - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) 這一步 - 希望 ackEnum 可以擺到正確的位置 - node 增加 GNSS 訊息 (2d fix 3d fix rtk float...) RTK - 跟 RTK2go 抓取列表 - 從特定 mount point 得到數據 - 做一個 ros2 service(action?) 接到數據並包裝 + 跟 RTK2go 抓取列表 Done + 從特定 mount point 得到數據 Done + 做一個 ros2 service 接到數據並包裝 下一步 - NODE 重啟 - 可以 refresh node topic + 重啟做好了 但是還沒自動化 下下一步 - + 整合 xbee api-api 功能 後面 - rssi 資訊提取s -mavlink timeout - CommLONG 這個是 mavlink bus 沒有回應 可能是丟包了 自己的常用指令 @@ -39,8 +32,18 @@ python -m someotherpkg.src.example_change_mode python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpoint:=No1bio_02 -p username:=chiyu1468@hotmail.com +python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart + +export ROS_DOMAIN_ID=13 +ros2 daemon stop +ros2 daemon start + ros2 topic list ros2 topic echo /fc_network/vehicle/sys3/battery +ros2 topic echo hz /fc_network/vehicle/sys3/battery + +ros2 topic bw /fc_network/vehicle/sys3/attitude +ros2 topic hz /fc_network/vehicle/sys3/attitude /home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7 @@ -51,7 +54,7 @@ ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5 ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}" ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}" -ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}" +ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 10, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}" ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}" @@ -60,10 +63,4 @@ sudo tcpdump -i lo 'udp dst port 14561' -X sudo tcpdump -i lo 'udp dst port 14550' -X -vv sudo tcpdump -i lo -X udp port 14550 -幾個要討論的 -1. 專案文件中 .mat 檔案是幹嘛的? -2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉 -3. readme 那串文字來源? 用途? -4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置 -5. pitch yaw roll 出來了 弄一下 -6. +