test_stream_6dof_example.py 3.05 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
"""
    Streaming 6Dof from QTM
"""

import asyncio
import xml.etree.ElementTree as ET
import pkg_resources

import qtm

QTM_FILE = pkg_resources.resource_filename("qtm", "data/Demo.qtm")


def create_body_index(xml_string):
    """ Extract a name to index dictionary from 6dof settings xml """
    xml = ET.fromstring(xml_string)

    body_to_index = {}
    for index, body in enumerate(xml.findall("*/Body/Name")):
        body_to_index[body.text.strip()] = index

    return body_to_index


async def main():

    # Connect to qtm
    connection = await qtm.connect("10.42.0.15")

    # Connection failed?
    if connection is None:
        print("Failed to connect")
        return

    # Take control of qtm, context manager will automatically release control after scope end
    async with qtm.TakeControl(connection, "password"):

        realtime = True

        if realtime:
            # Start new realtime
            temp_connection_state = await connection.get_state()

            print( "Connection State = " , temp_connection_state )

            if (temp_connection_state != qtm.QRTEvent.EventConnected):
                await connection.new()

        else:
            # Load qtm file
            await connection.load(QTM_FILE)

            # start rtfromfile
            await connection.start(rtfromfile=True)

    # Get 6dof settings from qtm
    xml_string = await connection.get_parameters(parameters=["6d"])

    print( "DEBUG 2" , xml_string )
    
    body_index = create_body_index(xml_string)

    print( "Body Index = " , body_index )

    #wanted_body = "L-frame"
    wanted_body = "CF12"

    def on_packet(packet):
        
        #info, bodies = packet.get_6d()
        info, bodies  = packet.get_6d_euler()

        print(
            "\n\n\nFramenumber: {} - Body count: {}".format(
                packet.framenumber, info.body_count
            )
        )

        print( "\nBodies = " , bodies )

        #print( "\nEuler = " , data_euler )

        print( "\n" )
        if info.body_count > 0:

            if wanted_body is not None and wanted_body in body_index:
                # Extract one specific body
                wanted_index = body_index[wanted_body]
                position, rotation = bodies[wanted_index]
                #print("{} - Pos: {} - Rot: {}".format(wanted_body, position, rotation))
                print("{} - Pos: {}".format(wanted_body, position))
                print("{} - Rot: {}".format(wanted_body, rotation))
            #else:
                # Print all bodies
                #for position, rotation in bodies:
                #    print("Pos: {} - Rot: {}".format(position, rotation))


    # Start streaming frames
    #await connection.stream_frames(components=["6d"], on_packet=on_packet)
    await connection.stream_frames(components=["6deuler"], on_packet=on_packet)

    # Wait asynchronously 5 seconds
    await asyncio.sleep(5)

    # Stop streaming
    await connection.stream_frames_stop()


if __name__ == "__main__":
    # Run our asynchronous function until complete
    asyncio.get_event_loop().run_until_complete(main())