nokov_python_crazyfile

[Felix-kkk]

I want to send the data to the mocap system to crazyfile through the lambda pose: send_extpose_quat() function, and then control it through the PositionHlCommander controller, but it doesn't work, can you give me an answer, thank you very much

`from nokov.nokovsdk import *
import time
import sys
from threading import Thread

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.mem import MemoryElement
from cflib.crazyflie.mem import Poly4D
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.utils import uri_helper
from cflib.positioning.position_hl_commander import PositionHlCommander
# URI to the Crazyflie to connect to
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')

# True: send position and orientation; False: send position only
send_full_pose = True

# 初始化变量
preFrmNo = 0
curFrmNo = 0
global client

# 1. 初始化Nokov动捕系统并连接
def initialize_nokov(server_ip='10.1.1.198'):
    global client
    client = PySDKClient()

    ver = client.PyNokovVersion()
    print('NokovSDK Sample Client 2.4.0.5428(NokovSDK ver. %d.%d.%d.%d)' % (ver[0], ver[1], ver[2], ver[3]))
    
    print("Initializing Nokov SDK Client...")
    ret = client.Initialize(bytes(server_ip, encoding="utf8"))
    if ret == 0:
        print("Connected to Nokov successfully.")
    else:
        print("Failed to connect to Nokov. Exiting.")
        sys.exit(1)

# 2. 设置动捕数据回调函数
def py_data_func(pFrameOfMocapData, pUserData):
    if pFrameOfMocapData == None:  
        print("Not get the data frame.\n")
    else:
        frameData = pFrameOfMocapData.contents
        global preFrmNo, curFrmNo 
        curFrmNo = frameData.iFrame
        if curFrmNo == preFrmNo:
            return
        global client
        preFrmNo = curFrmNo

        length = 128
        szTimeCode = bytes(length)
        client.PyTimecodeStringify(frameData.Timecode, frameData.TimecodeSubframe, szTimeCode, length)
        print(f"\nFrameNo: {frameData.iFrame}\tTimeStamp:{frameData.iTimeStamp}\t Timecode:{szTimeCode.decode('utf-8')}")					
        print( f"MarkerSet [Count={frameData.nMarkerSets}]")

        for iMarkerSet in range(frameData.nMarkerSets):
            markerset = frameData.MocapData[iMarkerSet]
            print( f"Markerset{iMarkerSet+1}: {markerset.szName.decode('utf-8')} [nMarkers Count={markerset.nMarkers}]")
            print("{")

            for iMarker in range(markerset.nMarkers):
                print(f"\tMarker{iMarker+1}(mm) \tx:{markerset.Markers[iMarker][0]:6.2f}"\
                    f"\ty:{markerset.Markers[iMarker][1]:6.2f}\tz:{markerset.Markers[iMarker][2]:6.2f}")
            print( "}")

        print(f"Markerset.RigidBodies [Count={frameData.nRigidBodies}]")
        for iBody in range(frameData.nRigidBodies):
            body = frameData.RigidBodies[iBody]
            print("{")
            print(f"\tid:{body.ID}")
            print(f"\t    (mm)\tx:{body.x:6.2f}\ty:{body.y:6.2f}\tz:{body.z:6.2f}")
            print(f"\t\t\tqx:{body.qx:6.2f}\tqy:{body.qy:6.2f}\tqz:{body.qz:6.2f}\tqw:{body.qw:6.2f}")		
		
            # Markers
            print(f"\tRigidBody markers [Count={body.nMarkers}]")
            for i in range(body.nMarkers):
                marker = body.Markers[i]
                print(f"\tMarker{body.MarkerIDs[i]}(mm)\tx:{marker[0]:6.2f}\ty:{marker[1]:6.2f}\tz:{marker[2]:6.2f}")
            print("}")  
    return  body.ID, body.x, body.y, body.z, body.qx, body.qy, body.qz, body.qw

        

# 3. 创建动捕系统包装类，读取数据
class MocapWrapper(Thread):
    def __init__(self, server_ip='10.1.1.198'):
        Thread.__init__(self)
        self._stay_open = True
        self.on_pose = None
        self.server_ip = server_ip
        self.start()

    def close(self):
        self._stay_open = False

    def run(self):
        # Initialize and connect to the Nokov system
        initialize_nokov(self.server_ip)

        # Set data callback
        #当客户端接收到新的数据帧时，py_data_func 会被自动调用
        [ID, x, y, z, qx, qy, qz, qw]=client.PySetDataCallback(py_data_func, None)
        q = [qx,qy,qz,qw]
        # Loop to receive data
        while self._stay_open:
            if self.on_pose:
                pos = [x, y, z, q]
                self.on_pose([pos[0], pos[1], pos[2], pos[3]])

def send_extpose_quat(cf, x, y, z, quat):
    """
    Send the current Crazyflie X, Y, Z position and attitude as a quaternion.
    This is going to be forwarded to the Crazyflie's position estimator.
    """
    if send_full_pose:
        cf.extpos.send_extpose(x, y, z, quat[0], quat[1], quat[2], quat[3])
    else:
        cf.extpos.send_extpos(x, y, z)

# 4. 主程序控制部分
if __name__ == '__main__':
    print("Initializing Crazyflie drivers...")
    cflib.crtp.init_drivers()
    print("Crazyflie drivers initialized.")
    
    # Set up the motion capture system handler
    mocap_wrapper = MocapWrapper(server_ip='10.1.1.198')  # Set the desired server IP

    # Crazyflie control loop
    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
        #基于全局坐标系和绝对位置
        cf = scf.cf
        with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc:
            mocap_wrapper.on_pose = lambda pose: send_extpose_quat(cf, pose[0], pose[1], pose[2], pose[3])
            # Adjust flying parameters here (e.g., sensitivity, trajectory)
            # Example: adjust_orientation_sensitivity(cf)
            #pc.take_off()
            pc.go_to(0.0, 0.0, 1.0)
            # Additional control logic for Crazyflie can be added here
        
            # Enable the Kalman estimator, upload trajectory, etc.
            # Example: activate_kalman_estimator(cf)
            # Example: upload_trajectory(cf, trajectory_id, figure8)
            print("Control loop running with motion capture data.")

    # Close the motion capture data processing
    mocap_wrapper.close()
`

[Bananazooka]

Hi,
You've closed this, does it mean you solved it?