server_udp.py 4.61 KB
Newer Older
Kamal Nasser's avatar
Kamal Nasser committed
1
2
3
4
5
import optirx as rx
import optirx_utils
import eventlet
import math
from oscpy.client import OSCClient
Kamal Nasser's avatar
Kamal Nasser committed
6
from utils import clip_value, remap_range
Kamal Nasser's avatar
Kamal Nasser committed
7
8
import json

Kamal Nasser's avatar
Kamal Nasser committed
9
eventlet.monkey_patch(all=True)
Kamal Nasser's avatar
Kamal Nasser committed
10

Kamal Nasser's avatar
Kamal Nasser committed
11
UDP_IP = "127.0.0.1"
Kamal Nasser's avatar
Kamal Nasser committed
12
13
14
UDP_PORT = 5000

max_udp_client = OSCClient(UDP_IP, UDP_PORT)
15
16


Kamal Nasser's avatar
Kamal Nasser committed
17
18
19
def send_udp(message):
    max_udp_client.send_message("/", message)

20

Kamal Nasser's avatar
Kamal Nasser committed
21
AXIS_RANGES = {
Kamal Nasser's avatar
Kamal Nasser committed
22
23
24
    'x': (-0.941, 0.972),
    'y': (-1.312, 0.768),
    'z': (-1.356, 0.779),
Kamal Nasser's avatar
Kamal Nasser committed
25
    'roll': (-180, 180)
Kamal Nasser's avatar
Kamal Nasser committed
26
}
Kamal Nasser's avatar
Kamal Nasser committed
27

Kamal Nasser's avatar
Kamal Nasser committed
28

29
30
31
def axis_length(axis):
    values = AXIS_RANGES[axis]
    return abs(values[1] - values[0])
Kamal Nasser's avatar
Kamal Nasser committed
32
33


34
35
36
def axis_midpoint(axis):
    values = AXIS_RANGES[axis]
    return (values[1] + values[0]) / 2
Kamal Nasser's avatar
Kamal Nasser committed
37

38
39

def get_zero_bounding_rect():
Kamal Nasser's avatar
Kamal Nasser committed
40
41
42
43
    offsets = {}
    for axis in ["x", "y", "z"]:
        length = axis_length(axis)
        midpoint = axis_midpoint(axis)
44
45
        offset = length * 0.15
        print "[zero] axis=%s offset=%f length=%f" % (axis, offset, length)
Kamal Nasser's avatar
Kamal Nasser committed
46
47
48
49
        offsets[axis] = (midpoint - offset, midpoint + offset)

    return offsets

50

Kamal Nasser's avatar
Kamal Nasser committed
51
52
53
ZERO_BOUNDING_RECT = get_zero_bounding_rect()
print(ZERO_BOUNDING_RECT)

54

Kamal Nasser's avatar
Kamal Nasser committed
55
56
57
58
59
60
61
62
def inside_zero_rect(p):
    for i, axis in enumerate(["x", "y", "z"]):
        rect = ZERO_BOUNDING_RECT[axis]
        if p[i] < rect[0] or p[i] > rect[1]:
            return False

    return True

Kamal Nasser's avatar
Kamal Nasser committed
63

64
65
66
67
68
69
70
71
72
73
def remap_and_clip(source_low, source_high, destination_low, destination_high, value):
    # clip Optritrack param to the limits of its defined valid range
    value = clip_value(value, source_low, source_high)
    # translate Optritrack param to rtscs param
    value = remap_range(value,
                            source_low, source_high,
                            destination_low, destination_high)

    return value

Kamal Nasser's avatar
Kamal Nasser committed
74

75
76
77
def remap_value_of_axis_range(axis, destination_low, destination_high, value):
    source_low = AXIS_RANGES[axis][0]
    source_high = AXIS_RANGES[axis][1]
Kamal Nasser's avatar
Kamal Nasser committed
78

79
    return remap_and_clip(source_low, source_high, destination_low, destination_high, value)
Kamal Nasser's avatar
Kamal Nasser committed
80
81


Kamal Nasser's avatar
Kamal Nasser committed
82
83
def zero_range_params(p):
    if inside_zero_rect(p):
Kamal Nasser's avatar
z axis    
Kamal Nasser committed
84
        #print "inside"
85
        return [0.0, 0.0, 0.0]
Kamal Nasser's avatar
Kamal Nasser committed
86

Kamal Nasser's avatar
Kamal Nasser committed
87
    return p
Kamal Nasser's avatar
Kamal Nasser committed
88

89

Kamal Nasser's avatar
Kamal Nasser committed
90
91
92
93
94
def get_rtscs_params_body(rh):
    """
    Get input from the OptiTrack system streaming engine, transform the data to rtscs_params and return it.
    """
    if rh is None or rh.mrk_mean_error == 0:
Kamal Nasser's avatar
Kamal Nasser committed
95
        return 0, 0, 0, 0
Kamal Nasser's avatar
Kamal Nasser committed
96
97
98
99

    # mul by -1 to fix flipped coordinates if not fully compatible Motive calibration square
    rh_position = list(rh.position)  # map(lambda coordinate: -1 * coordinate, rh.position)
    # Convert right hand convention to left hand convention for Motive 1.73+ used with CS-200
Kamal Nasser's avatar
Kamal Nasser committed
100
    rh_position[0] = -rh_position[0]
Kamal Nasser's avatar
Kamal Nasser committed
101
102
103
    # For convenience convert roll angle from radians to degrees
    rh_roll = math.degrees(optirx_utils.orientation2radians(rh.orientation)[0])

Kamal Nasser's avatar
Kamal Nasser committed
104
105
    rh_position = zero_range_params(rh_position)

106
    return rh_position + [rh_roll]
Kamal Nasser's avatar
Kamal Nasser committed
107

Kamal Nasser's avatar
Kamal Nasser committed
108
109
110

udp_msg_to_send = None

111
112

xAxisLength = axis_length("x")
Kamal Nasser's avatar
z axis    
Kamal Nasser committed
113
zAxisMidpoint = axis_midpoint("z") + axis_length("z")/4
114
115


Kamal Nasser's avatar
Kamal Nasser committed
116
117
def get_optirx_data():
    dsock = rx.mkdatasock(ip_address="127.0.0.1", multicast_address='239.255.42.99', port=1511)
Kamal Nasser's avatar
Kamal Nasser committed
118
    natnetsdk_version = (2, 9, 0, 0)
Kamal Nasser's avatar
Kamal Nasser committed
119
120
121
122
123
124
    while True:
        data = dsock.recv(rx.MAX_PACKETSIZE)
        # parse packet and store it locally
        packet = rx.unpack(data, natnetsdk_version)
        if type(packet) is rx.SenderData:
            natnetsdk_version = packet.natnet_version
Kamal Nasser's avatar
Kamal Nasser committed
125
126
127
128
129
130
131
132
133
        bodies = optirx_utils.get_all_rigid_bodies(packet)
        if len(bodies) == 0:
            continue

        b1 = bodies[0]
        b2 = bodies[1]
        if b1 is not None and b2 is not None:
            params1 = get_rtscs_params_body(b1)
            params2 = get_rtscs_params_body(b2)
134

Kamal Nasser's avatar
Kamal Nasser committed
135
            if params1 != (0, 0, 0, 0) and params2 != (0, 0, 0, 0):
136
                xDistance = params1[0] - params2[0]
Kamal Nasser's avatar
z axis    
Kamal Nasser committed
137
                berlinPos = params1[2]
Kamal Nasser's avatar
Kamal Nasser committed
138
                msg = [json.dumps({
139
140
                    'drumVolume': remap_value_of_axis_range("y", 0.0, 1.0, params1[1]),
                    'groupCVolume': remap_value_of_axis_range("y", 0.0, 1.0, params2[1]),
Kamal Nasser's avatar
z axis    
Kamal Nasser committed
141
142
                    'tempo': int(remap_and_clip(-xAxisLength, xAxisLength, 60, 132, xDistance)),
                    'berlin': 0 if berlinPos < zAxisMidpoint else int(remap_value_of_axis_range("z", 0, 50, params1[2]))
Kamal Nasser's avatar
Kamal Nasser committed
143
                })]
144

Kamal Nasser's avatar
Kamal Nasser committed
145
146
                global udp_msg_to_send
                udp_msg_to_send = msg
147

Kamal Nasser's avatar
Kamal Nasser committed
148
149
150
151
152
153
154
155

def send_udp_interval():
    while True:
        global udp_msg_to_send
        if udp_msg_to_send != None:
            print "sending %s" % udp_msg_to_send
            send_udp(udp_msg_to_send)
            udp_msg_to_send = None
Kamal Nasser's avatar
Kamal Nasser committed
156

Kamal Nasser's avatar
Kamal Nasser committed
157
158
159
160
        eventlet.sleep(0.05)


eventlet.spawn(send_udp_interval)
Kamal Nasser's avatar
Kamal Nasser committed
161
t_data = eventlet.spawn(get_optirx_data)
Kamal Nasser's avatar
Kamal Nasser committed
162
163


Kamal Nasser's avatar
Kamal Nasser committed
164
t_data.wait()