wenchun/AirTrapMine
1
0
Fork 0
forked from chiyu1468/AirTrapMine
Code Pull Requests Activity

node重啟功能 與 定義檔優化

Browse Source 
---
(Adding)
1. mavlinkROS2Nodes.py 各別 node 重啟的功能
2.  mainRochestrator.py 開發對應介面
(modify)
 1. 定義檔優化 移除 ackEnum.py 新增 ServiceAckResult.msg
This commit is contained in:
chiyu1468
2026-05-30 15:15:25 +08:00
parent 4e08b18a03
commit 61e283cd25
8 changed files with 281 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+9 -3
View File
@@ -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
===
## 運行環境
src/fc_interfaces/CMakeLists.txt
+1
View File
@@ -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"
src/fc_interfaces/msg/ServiceAckResult.msg
+43
View File
@@ -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
src/fc_network_adapter/fc_network_adapter/ackEnum.py
-31
View File
@@ -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
src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
+8 -1
View File
@@ -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指令
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+71 -12
View File
@@ -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()))
@@ -569,7 +582,39 @@ class ControlPanel:
continue # 只有在工程模式下才能操作
cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
elif selected.action == "INSPECT_VEHICLES":
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 == "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 重啟的功能
'''
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
+127 -26
View File
@@ -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
@@ -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)
@@ -1209,6 +1214,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,
daemon=True,
@@ -1224,21 +1233,102 @@ class fc_ros_manager:
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)
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:
@@ -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,
@@ -1299,6 +1389,12 @@ class fc_ros_manager:
}
# ============================================================================
# 全域實例
# ============================================================================
@@ -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 中斷了 如何恢復的問題
'''
src/unitdev02/unitdev02/devnote.txt
+17 -20
View File
@@ -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.