collision-detection/map_tp.py at master · class-euproject/collision-detection · 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
print("before imports")

import time
import os

#from tp.dataclayObjectManager import DataclayObjectManager
from tp.v3TP_new import traj_pred_v3


from tp.v3TP_new import QUAD_REG_LEN_DICT


import paho.mqtt.client as mqtt
print("after imports")

dm = None


def traj_pred_v2_wrapper(objects_chunk):
  try:
    print(" = ==================in new_traj_pred_v2_wrapper!!! ===============")
    print(f"with input: {objects_chunk}")
    global dm
    if not dm:
      print(f"creating DATAMANAGER INSTANce")
      from tp.dataclayObjectManager import DataclayObjectManager
      dm = DataclayObjectManager()
    print(f"objects in chunk: {len(objects_chunk)}")
    print(objects_chunk)
    print(objects_chunk[0])
    print('---')
    if isinstance(objects_chunk[0], int):
        print("steam up, return")
        time.sleep(0.3)
        return

    for objectTuple in objects_chunk:
        print(f"before dm.getObject: {objectTuple[0]}")

        # calculate trajectory by v2
        print(f"before traj_pred_v3:{objectTuple[0]}")
        fx, fy, ft = traj_pred_v3(objectTuple[5][0], objectTuple[5][1], objectTuple[5][2], objectTuple[8], objectTuple[9], objectTuple[6].split('_')[0])

        print(f"v_id: {objectTuple[0]} x: {fx} y: {fy} t: {ft}")

        tp_timestamp = objectTuple[5][2][-1]

        try:
            obj = dm.getObject(objectTuple[0])
            dm.storeResult(obj, fx, fy, ft, tp_timestamp, objectTuple[7])
        except Exception as e:
            import traceback
            traceback.print_exc()
            print("Got some bad DC exception, ignoring")

            client=mqtt.Client()
            client.connect("192.168.7.42")
            client.publish("test", f"Got some bad DC exception, ignoring. AID: {os.environ['__OW_ACTIVATION_ID']}")

            print("----TP objects--------")
            print(f'fx: {fx} fy: {fy} ft: {ft}')

            print("----TP objects types--------")
            print(f'fx: {type(fx)} fy: {type(fy)} ft: {type(ft)}')

            print("----TP objects list items types--------")
            print(f'fx: {type(fx[0])} fy: {type(fy[0])} ft: {type(ft[0])}')

            print("----------------------------")


        print(f"after dm.storeResult:{objectTuple[0]}")

    print(" = ==================out traj_pred_v2_wrapper ===============")
  except Exception as e:
    print("Got exception")
    client=mqtt.Client()
    client.connect("192.168.7.42")
    client.publish("test", f"Got an unknown exception in TP, raising. AID: {os.environ['__OW_ACTIVATION_ID']}")
    raise e