AirTrapMine/src/unitdev04/test01.py

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