QualisysDataPublisherWIP.py 4.32 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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

#    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
#
#    This file is part of D-FaLL-System.
#    
#    D-FaLL-System is free software: you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation, either version 3 of the License, or
#    (at your option) any later version.
#    
#    D-FaLL-System is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#    
#    You should have received a copy of the GNU General Public License
#    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
#    
#
#    ----------------------------------------------------------------------------------
#    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
#    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
#    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
#    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
#    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
#
#
#    DESCRIPTION:
#    The publisher for the Qualisys motion capture data
#
#    ----------------------------------------------------------------------------------




import roslib; roslib.load_manifest('dfall_pkg')
import rospy


import rosbag
from rospkg import RosPack



# Import the library for asynchronous I/O
import asyncio

# Import the Qualisys Track Manager (QTM) Library
import qtm


def on_packet_3d(packet):
    """ Callback function that is called everytime a data packet arrives from QTM """
    print("Framenumber: {}".format(packet.framenumber))
    header, markers = packet.get_3d_markers()
    print("Component info: {}".format(header))
    for marker in markers:
        print("\t", marker)


def on_packet_6d(packet):
        info, bodies = packet.get_6d()
        print(
            "Framenumber: {} - Body count: {}".format(
                packet.framenumber, info.body_count
            )
        )

        print( "Bodies = " , bodies )

        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))
            else:
                # Print all bodies
                for position, rotation in bodies:
                    print("Pos: {} - Rot: {}".format(position, rotation))


async def setup():
    """ Main function """
    connection = await qtm.connect("10.42.0.15")

    if connection is None:
        print("[QUALISYS DATA PUBLISHER] connection is NONE")
        return

    async with qtm.TakeControl(connection, "password"):


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

        print( "[QUALISYS DATA PUBLISHER] connection State = " , temp_connection_state )

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

    print( "DEBUG 1" )

    # 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( "DEBUG 3" )

    print( "Body Index = " , body_index )

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

    print( "DEBUG 5" )

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

    #await connection.stream_frames(components=["3d"], on_packet=on_packet_3d)


if __name__ == "__main__":
	# Starting the ROS-node
    global node_name
    node_name = "QualisysDataPublisherWIP"
    rospy.init_node(node_name, anonymous=True)

    # Get the namespace of this node
    global ros_namespace
    ros_namespace = rospy.get_namespace()

    asyncio.ensure_future(setup())
    asyncio.get_event_loop().run_forever()

    # PAUSE FOR A SECOND AND THEN SPIN
    time.sleep(1.0)
    print("[QUALISYS DATA PUBLISHER] Node READY :-)")
    rospy.spin()