temp commit for share code

README.md

@@ -16,5 +16,11 @@
 
 === 
 Package 簡述
+1. unitdev 為各自協作者做開發時的測試區
+2. fc_network_adapter 
+    建立、維護與飛控韌體的連接 
+    構築 mavlink 封包
+    同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息
+    處理無線模組的通訊格式 (XBee)
 
 

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -0,0 +1,97 @@
+import threading
+from pymavlink import mavutil
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+import time
+
+'''
+
+# 不同的匯流排只有單一種通訊協定 
+# 匯流排接到訊息後透過 queue stack 傳送到分析器
+# 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器
+# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue
+# 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去
+
+# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排
+# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面
+# 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function
+
+'''
+
+class MavlinkObject():
+    def __init__(self, mavlink_socket):
+        self.mavlink_socket = mavlink_socket
+        self.msg_count = {}
+        # self.multiplexingDict = {}
+        self.multiplexingToAnalysis = []
+        self.multiplexingToReturn = []
+        self.multiplexingToConvert = []
+    def __del__(self):
+        self.mavlink_socket.close()
+        self.stop()
+
+    def run(self):
+        self.thread = threading.Thread(target=self._run_thread)
+        self.running = True
+        self.thread.start()
+    def stop(self):
+        self.running = False
+
+    def _run_thread(self):
+        start_time = time.time()
+        while self.running:
+            try:
+                mavlinkMsg = self.mavlink_socket.recv_msg()
+                if mavlinkMsg:
+                    self.count_mavlink_type(mavlinkMsg)
+                    # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug
+                    # break # Debug
+                    
+            except Exception as e:
+                print(f"Error receiving mavlink data: {e}")
+                print(mavlinkMsg)
+                self.running = False
+                break
+
+            # 每秒中 統計一次 收到的訊息總量
+            elapsed_time = time.time() - start_time
+            if elapsed_time > 1:
+                print('Messages Type received (in about 1 sec) :')
+                print(self.msg_count)
+                start_time = time.time()
+                self.msg_count = {}
+
+    def count_mavlink_type(self, mavlinkMsg):
+        msg_type = mavlinkMsg.get_type()
+        if msg_type in self.msg_count:
+            self.msg_count[msg_type] += 1
+        else:
+            self.msg_count[msg_type] = 1
+
+    def publish_mavlink_data(self, mavlinkMsg):
+        msg = String()
+        msg.data = str(mavlinkMsg)
+        self.publisher_.publish(msg)
+
+
+
+def main(args=None):
+
+    connection_string="udp:127.0.0.1:14550"
+    mavlink_socket = mavutil.mavlink_connection(connection_string)
+
+    mavlink_object = MavlinkObject(mavlink_socket)
+    mavlink_object.run()
+
+    time.sleep(5)
+    mavlink_object.running = False
+    mavlink_object.thread.join()
+    print('End of Program')
+
+
+
+if __name__ == '__main__':
+    main()

src/fc_network_adapter/fc_network_adapter/socketManager.py

@@ -0,0 +1,14 @@
+
+'''
+
+透過某個地方 得到 udp 或 uart 接口
+對於每個接口 視為一個獨立的物件 
+
+物件對於不同的接口 是為不同類型的物件
+
+每個類型的物件 創建一個獨立的執行緒 來處理資料
+關於執行緒的實作 是寫在另一個模組
+
+物件之間 也可以做資料轉換/轉拋
+
+'''

src/fc_network_adapter/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fc_network_adapter</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/fc_network_adapter/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/fc_network_adapter
+[install]
+install_scripts=$base/lib/fc_network_adapter

src/fc_network_adapter/setup.py

@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = 'fc_network_adapter'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='picars',
+    maintainer_email='chiyu1468@hotmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

src/unitdev02/unitdev02/test01.py

@@ -42,6 +42,11 @@ def mavlink_analyzer_simple(count = 500):
             print('No message yet, 1 second for data to fill')
             sleep(1)
 
+    print("markA ------")
+    print(msg.get_type())
+    print(msg.ordered_fieldnames)
+    print("markA ------ END")
+
     print('Messages Type received:')
     print(msg_count)
 
@@ -108,7 +113,8 @@ def fdm_switch_example_one():
         elapsed_time = current_time - start_time
 
         if elapsed_time >= 1.0:
-            print(f"Packets received in the last second: {packet_count}")
+            # print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack : {data_g2s}')
             packet_count = 0
             start_time = current_time
             fdm_parser_example(data_s2g)
@@ -132,24 +138,24 @@ def fdm_switch_example_two():
     server_address = ('127.0.0.1', 19002)
 
     data_queue = queue.Queue()
-    servo_out = [0] * 16
+    servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....]
     data_queue.put(servo_out)
 
-    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=10, frame_count_fdm=1):
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
         interval = 1.0 / frame_rate_fdm
         while True:
             start_time = time.time()
             try:
-                data = q.get(timeout=0.1)
+                data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
                 if data is None:
                     break
                 last_data = data
             except queue.Empty:
                 data = last_data
-            data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
-            sock.sendto(data, server_address)
-            frame_count_fdm += 1
-            fdm_parser_example(data) # debug
+            data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
+            sock.sendto(data_fdm, server_address)
+            frame_count_fdm += 3
+            # fdm_parser_example(data_fdm) # debug
 
             elapsed_time = time.time() - start_time
             sleep_time = interval - elapsed_time
@@ -159,6 +165,7 @@ def fdm_switch_example_two():
     Running = True
     thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
     thread.start()
+    print('Start sending data to Gazebo')
 
     while Running:
         msgb1 = None
@@ -184,7 +191,39 @@ def fdm_switch_example_two():
         # To stop the thread, you can put None into the queue
         # data_queue.put(None)
 
+from pymavlink.dialects.v20 import common  # 使用 MAVLink 2.0
+
+def mavlink_btye_generator_test():
+
+    # 創建一個 MAVLink 對象
+    mav = common.MAVLink(None)  # 不透過 connection，直接使用 None
+
+    # 創建一個 HEARTBEAT 訊息
+    msg = mav.heartbeat_encode(
+        type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
+        autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+        base_mode=0,
+        custom_mode=0,
+        system_status=mavutil.mavlink.MAV_STATE_ACTIVE
+    )
+
+    # 取得 MAVLink 訊息的 bytes
+    mavlink_bytes = msg.pack(mav)
+
+    # 回傳 bytes
+    print(mavlink_bytes)
+    print(type(mavlink_bytes))
+
+    mav2 = common.MAVLink(None)
+    # my_msg = mav2.decode(mavlink_bytes)
+    print("========")
+    print(dir(mav2))
+    # print(my_msg)
+
+
 
 print('Start')
 # fdm_switch_example_two()
 # fdm_parser_example()
+# mavlink_analyzer_simple(8)
+mavlink_btye_generator_test()