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()