MageGuide/wandInterface.py at main · charles-rothbaum/MageGuide · GitHub

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
# get the coordinates of want tip from mocap system
# perform scaling operations
# prepare to send instructions to drone

from pymavlink import mavutil
from Custom_Mocap_Commands import *
from Custom_Drone_Commands import *
from Custom_Drone_Commands_Gazebo import *
import time
import threading

class mocap_streaming_thread(threading.Thread):
    def __init__(self, thread_name, thread_ID, mocap_connection, init_time):
        threading.Thread.__init__(self)
        self.setDaemon(True)
        self.thread_name = thread_name
        self.thread_ID = thread_ID
        self.mocap_connection = mocap_connection
        self.init_time = init_time
        self.wand_pos = None
        self.wand_rot = None

    def run(self):
        while True:
            time.sleep(.1)

            [self.wand_pos, self.wand_rot] = self.mocap_connection.rigid_body_dict[2]

            #print(f"Current y (m): {self.wand_pos[1]}")
            #print(f"Current z (m): {self.wand_pos[0]}")
            #print(f"Current x (m): {self.wand_pos[2]}")


        return


#Main thread:

init_time = time.time()

streaming_client = mocap_connect()
is_running = streaming_client.run()

stream = mocap_streaming_thread("stream1", 1, streaming_client, init_time)
stream.start()

drone_connection = connect(14551)
takeoff(drone_connection, 1)
time.sleep(5)

while(is_running):
    print(f"current pos (m): {stream.wand_pos}")
    send_waypoint_local(drone_connection, 5 * stream.wand_pos[2], -5 * stream.wand_pos[0], -1 * stream.wand_pos[1])
    time.sleep(.5)