From 415cf1585f2c40932ea35b9eb43c55fdcfa6b527 Mon Sep 17 00:00:00 2001 From: lenting89 Date: Tue, 28 Apr 2026 15:34:07 +0800 Subject: [PATCH] update before merging --- src/unitdev04/test01.py | 229 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 229 insertions(+) create mode 100644 src/unitdev04/test01.py diff --git a/src/unitdev04/test01.py b/src/unitdev04/test01.py new file mode 100644 index 0000000..bf634e2 --- /dev/null +++ b/src/unitdev04/test01.py @@ -0,0 +1,229 @@ +import rclpy +from pymavlink import mavutil +from time import sleep +import time + +import socket +import struct + +# used at fdm_switch_example_two +import threading +import queue + +def mavlink_analyzer_simple(count = 500): + # rclpy.init() + # node = rclpy.create_node('mavlink_analyzer_simple') + + print('Inintializing connection') + + connection_string="udp:127.0.0.1:14550" + connection = mavutil.mavlink_connection(connection_string) + + print('Catch messages') + msg_count = {} + + start_time = time.time() + + for _ in range(count): + msg = connection.recv_msg() + # msg = connection.recv_match(blocking=True) + if msg: + msg_type = msg.get_type() + if msg_type in msg_count: + msg_count[msg_type] += 1 + else: + msg_count[msg_type] = 1 + + if msg.get_type() == 'SERVO_OUTPUT_RAW': + print(msg) + pass + + else: + 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) + + end_time = time.time() + print('Time elapsed: ', end_time - start_time) + + print('Closing connection') + connection.close() + + +def fdm_parser_example(data=None): + if data is None: + data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000') + # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000 + + + parse_format = 'HHI16H' + + if len(data) != struct.calcsize(parse_format): + print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format))) + exit(1) + + decoded = struct.unpack(parse_format,data) + + magic = decoded[0] + frame_rate_hz = decoded[1] + frame_count = decoded[2] + pwm = decoded[3:] + + + print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm)) + +def fdm_switch_example_one(): + + ''' + 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 + 沒有轉換數據格式 + ''' + # make a link get fdm from SITL, as a server + sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock_s2g.bind(('', 9002)) + sock_s2g.settimeout(0.1) + + # set a target link pass JSON to Gazebo, as a client + sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_address = ('127.0.0.1', 19002) + + packet_count = 0 + start_time = time.time() + + while True: + try: + # 接到SITL的數據 並轉發給Gazebo + data_s2g, addr_s2g = sock_s2g.recvfrom(1024) + sock_g2s.sendto(data_s2g, server_address) + # 得到Gazebo的回應 並轉發給SITL + data_g2s, addr_g2s = sock_g2s.recvfrom(1024) + sock_s2g.sendto(data_g2s, addr_s2g) + packet_count += 1 + except socket.timeout: + pass + + current_time = time.time() + elapsed_time = current_time - start_time + + if elapsed_time >= 1.0: + # 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) + + +def fdm_switch_example_two(): + ''' + 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 + 有轉換數據格式 + + time_usec + servo1_raw + servo16_raw + ''' + # read info from SITL via MAVLink + connection_string="udp:127.0.0.1:14550" # uart mavlink # MATLAB + connection = mavutil.mavlink_connection(connection_string) + + # set a target link pass JSON to Gazebo, as a client + sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_address = ('127.0.0.1', 19002) + + data_queue = queue.Queue() + 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=1, frame_count_fdm=1): + interval = 1.0 / frame_rate_fdm + while True: + start_time = time.time() + try: + 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_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 + if sleep_time > 0: + sleep(sleep_time) + + 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 + msg = connection.recv_msg() + while msg : + if msg.get_type() == 'SERVO_OUTPUT_RAW': + msgb1 = msg + # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值 + msg = connection.recv_msg() + + if msgb1: + for i in range(16): + servo_out[i] = getattr(msgb1, f'servo{i+1}_raw') + data_queue.put(servo_out) + msgb1 = None + # Running = False # debug + else: + # print('No message yet, 10 ms for data to fill') + sleep(0.01) + + # Example of putting data into the queue + # data_queue.put(data) + # 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()