modify-wrapper/mfwrapped.py at master · modifly-technologies/modify-wrapper · 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
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
from time import sleep

import modipy
import time
import json
class MDrone:
    def __init__(self, name, simobj):
        self.start_time = None
        self.name = name
        self.simobj = simobj
        self.pos = None
        self.att = None
        self.waitForPos = False
        self.waitForAtt = False
    def Arm(self):
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "arm"})
    def Disarm(self):
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "disarm"})
    def Takeoff(self):
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "takeoff"})
    def Land(self):
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "land"})
    def Reset(self):
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "reset"})
    def SetPosition(self, x, y, z, yaw):
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "override_pos", "x" : x, "y" : y, "z" : z, "yaw" : yaw})
    def GetPosition(self):
        self.waitForPos = True
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "getPosition"})
        self.start_time = time.time()
        while self.waitForPos:
            if time.time() - self.start_time > 1:
                return self.GetPosition()
            self.simobj.RunSDK()
        return self.pos
    def SetVelocityBodySP(self, vx, vy, vz, yawRate):
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "setVelocityBody", "x" : vx, "y" : vy, "z" : vz, "yaw" : yawRate})
        pass
    def GetAttitude(self):
        self.waitForAtt = True
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "getAttitude"})
        while self.waitForAtt:
            self.simobj.RunSDK()
        return self.att
    def HandleMSG(self, msg):
        if msg["device"] != self.name:
            return
        if msg["action"] == "position":
            self.pos = msg
            self.waitForPos = False
        if msg["action"] == "attitude":
            self.att = msg
            self.waitForAtt = False
    def WaitForDevice(self):
        self.waitForPos = True
        self.simobj.SendOBJMsg({"device" : self.name, "action" : "getPosition"})
        self.start_time = time.time()
        while self.waitForPos:
            if time.time() - self.start_time > 1:
                return self.WaitForDevice()
            self.simobj.RunSDK()
        return self.pos

class MSimulation:
    def __init__(self):
        self.sim = modipy.SimState()
        self.sim.use_internal_controller()
        self.last_run_time = time.time() * 1000
        self.GetSDKObj().on_sim_msg_event(lambda pkg, msg: self._handle_incoming(msg))
        self.drones = []
        # self.Step(0.1, 1000)
    def AddDrone(self, name):
        self.sim.add_drone(name, False)
        drone = MDrone(name, self)
        self.drones.append(drone)
        return drone
    def Cleanup(self):
        self.sim.cleanup()
    def GetSDKObj(self):
        return self.sim.get_sdk()
    def LaunchCockpit(self):
        self.sim.launch_cockpit()
    def RunSDK(self):
        self.sim.get_sdk().run_once()
    def Step(self, amt, rate):
        if (time.time() * 1000) - self.last_run_time > (1/rate):
            self.GetSDKObj().step(amt)
            self.last_run_time = time.time() * 1000
            self.RunSDK()
    def _handle_incoming(self, msg):
        # print(msg)
        object = json.loads(msg)
        if object["simInfo"] != "MODIFLYSITL_WRAPPER":
            return
        for drone in self.drones:
            drone.HandleMSG(object)

    def IsRunning(self):
        if self.sim.is_cockpit_launched():
            if not self.sim.is_cockpit_running():
                return False
        return self.sim.is_controller_running()
    def SendOBJMsg(self, obj):
        obj["simInfo"] = "MODIFLYSITL_WRAPPER"
        self.GetSDKObj().send_sim_message("system", json.dumps(obj))