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 efd48cd..dc87053 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 @@ -6,6 +6,7 @@ # 開發此專案的注意事項 - 預設 autopilot 的 component id = 1 - 不允許 system id 重複 +- 預設同一載具僅會使用相同的 socket - 增加一個固定數值監控然後要到 ros2 topic - mavlinkROS2Node.py 檔案內 - PublishRateController.topic_intervals 建立 @@ -126,6 +127,7 @@ - *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) } - *self.mavlink_socket* 從 pymavlink 繼承的socket物件 - *self.state* 描述這個 socket 物件的狀態 +- *self.MAVLink* pymavlink 的一個裝置物件 模擬自己是某一個裝置 用以對於該 mavlink bus 負責封包發送 --- - *process_data()* [async method] 核心方法 - *remove_target_socket()* *add_target_socket()* @@ -170,6 +172,45 @@ ## mavlinkVehicleView.py 這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用 +## mavlinkROS2Nodes.py + 這裡支持了所有 ROS2 框架的對應功能 包涵 + 1. 將載具的串流更新的狀態更新到對應的 topic + 2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面 + +### **[Class]** fc_ros_manager + MAVLink ROS2 節點管理器 管理兩個獨立的 Node + 會開一個執行緒 讓兩個 Node 都跑在裡面 + 然後用 spin_once 保持 Node 的活性 +- *self.status_publisher* 物件實例 +- *self.command_service* 物件實例 +- *self.spin_thread* 執行緒 +--- +- *start()* 啟動自己 +- *stop()* 停下自己 +- *shutdown()* 關閉自己並清理 + +### **[Class]** VehicleStatusPublisher + 這整個組件都是自動的 目前沒有什麼需要 runtime 設定的地方 +- *self.fc_publishers* 儲存所有 publisher 的資料結構 +- *self.rate_controller* 儲存頻率參數的地方 也可以關掉某個 topic 的發佈 +--- +- *timer_callback()* 自動的抓出現有的 vehicle 中有的資訊 然後固定的將訊息丟到負責發佈的方法中 +- *_publish_vehicle_status()* 接 timer_callback 後去分配到各項發佈中 +- *_get_or_create_publisher()* 實際創建 topic 的地方 +- *_publish_XXX()* 各項發佈的方法 + +### **[Class]** MavlinkCommandService + 提供 ros2 service 讓 serial_object 去丟出 mavlink 封包 + 然後會從回應封包中挑出期待的回應 再傳給 ros2 request + 其中 PendingEntry 是很關鍵的東西 + 它的每個物件 代表需要等待的封包種類與必需包含的內容 +- *self._pending_by_sysid* 儲存 PendingEntry 的地方 +--- +- *return_router()* 檢查並消耗 return_packet_ring 然後將封包分配到 pending 去 +- *__init__()* 這邊登入要創建的 service 到 ros2 系統 +- *handle_XXX()* 這邊是實現 service 的具體方法 + + # 開發記錄 ## 已實現功能 @@ -182,9 +223,9 @@ 7. 終端機介面控制 8. 基礎載具流量觀測 9. 載具狀態收集與彙整 -10. a. ros2 topic 應用開發介面 +10. a. ros2 topic 應用開發介面 (基礎框架) + b. ros2 service 應用開發介面 (基礎框架) ### 待開發功能 5-1. 建立 serial 連線 並可以對接收器下達AT指令 -5-2. 模組化 serial 連線機制 以利後期擴容其他模組 -10. a. ros2 應用開發介面 \ No newline at end of file +5-2. 模組化 serial 連線機制 以利後期擴容其他模組 \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 694913c..ea07445 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -28,9 +28,6 @@ mavlink_bridge: 專門處理 stream_bridge_ring 裡面的訊息流 會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖 - - - ''' # 基礎功能的 import diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index eca0bcc..bb5be21 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -40,11 +40,11 @@ from .utils import setup_logger logger = setup_logger(os.path.basename(__file__)) - # ============================================================================ -# 頻率控制器 +# VehicleStatusPublisher Node # ============================================================================ +# 頻率控制器 class PublishRateController: """發布頻率控制器 - 按時間間隔控制發布頻率""" @@ -103,11 +103,6 @@ class PublishRateController: """重置所有計時器""" self.last_publish_time.clear() - -# ============================================================================ -# VehicleStatusPublisher Node -# ============================================================================ - class VehicleStatusPublisher(Node): """ 載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics @@ -477,12 +472,17 @@ class MavlinkCommandService(Node): - 作為 service server 等待 client 請求 - 接收請求,組裝 MAVLink 封包 - 調用 mavlinkObject 發送封包 - - 處理 ACK 等待和超時(未來實現) + - 處理 ACK 等待和超時 設計理念:回歸 MAVLink 純粹結構 - 只負責將 ROS2 請求轉換為 MAVLink 封包 - 不預設功能(如 ARM/DISARM) 保持模組化 - 高層應用可透過此 service 實現各種功能 + + 講白話一點就是 + 每次接到一個 service 請求 要整個系統丟某種指令給載具時 + 會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending" + """ serviceString_prefix = '/fc_network/vehicle' @@ -493,8 +493,8 @@ class MavlinkCommandService(Node): # 狀態標記 self.running = True - # mavlinkObject 的引用(將由外部設置) - self.mavlink_analyzer = None + # # mavlinkObject 的引用(將由外部設置) 用不到 + # self.mavlink_analyzer = None # pending 旗標物件的儲存庫 self._pending_by_sysid = {} # sysid(int) : PendingEntry @@ -557,7 +557,8 @@ class MavlinkCommandService(Node): def return_router(self): ''' - 這邊是給外部迴圈呼叫的 消耗 return_packet_ring 裡接收到的 mavlink 封包 + 在節點管理器哪邊被呼叫 + 消耗 return_packet_ring 裡接收到的 mavlink 封包 分送到各自的 pending 中 藉由 event.set() 解開 service 中的 block ''' @@ -953,7 +954,8 @@ class fc_ros_manager: try: # 初始化 ROS2 - rclpy.init() + if not rclpy.ok(): + rclpy.init(args=None) # 創建節點 node self.status_publisher = VehicleStatusPublisher() @@ -1095,5 +1097,9 @@ ros2_manager = fc_ros_manager() 1. 完成 ros2 的 MavPositionTargetGlobalInt 區域 2. 優化 response.ack_result 回傳值的資訊 + +TODO +1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期 +2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題 '''