forked from shinkansan/2019-UGRP-DPoom
-
Notifications
You must be signed in to change notification settings - Fork 0
/
DRIFT.py
119 lines (97 loc) · 3.05 KB
/
DRIFT.py
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
# Auto Drive integration
import threading
import numpy
#import banner
import rospy
rospy.init_node('robot_mvs', anonymous = False)
import os
import autodrive as ad
import ground_seg as morp
#from easygo import easyGo
from easygo import easyGo as eg
#import easyDict
import os
def _cls():
os.system("cls" if os.name == "nt" else "clear")
pass
# command set
slam_launch_command = '''roslaunch realsense2_camera opensource_tracking_tk_localization.launch'''
init_dpoom_command = '''sh init_dpoom.sh'''
handle_mode = 'none'
def handle_master(mode):
if mode == 'autodrive':
handle_mode = 'autodrive'
DRIFT_handling_authority = True
ad.DRIFT_handling_authority = DRIFT_handling_authority
morp.handle_easy = False
rospy.set_param('/dap_handle', True)
print('dap_handle :: ', rospy.get_param('/dap_handle'))
print('***MORP Handle Status ***' + str(morp.handle_easy))
else:
handle_mode = 'morp'
rospy.set_param('/dap_handle', False)
morp.handle_easy = True
print('***MORP Handle Status ***' + str(morp.handle_easy))
#GoEasy(morp.direc)
print('Current handle mode ', handle_mode)
#banner.banner(handle_mode, ['morp', morp.handle_easy, 'DRIFT', DRIFT_handling_authority, 'adDrift', ad.DRIFT_handling_authority])
def slam_launcher():
os.system(slam_launch_command)
def init_dpoom():
os.system(init_dpoom_command)
def auto_drive_launcher():
ad.d_main()
print('Desination Reached!!!')
raise('ERROR')
def morp_launcher():
morp.main()
pass
def morp_dap_transfer():
while True:
_cls()
print('MORP Report : ' + morp.currentStatus)
print('Localization : ', ad.flagLocal)
if morp.currentStatus == "YES OBSTACLE" and ad.flagLocal:
handle_master('morp')
else:
handle_master('autodrive')
def GoEasy(direc):
if direc == 0:
easyGo.mvCurve(-SPEED, 0)
elif direc == 1:
#print("COME HERE")
easyGo.mvCurve(SPEED, 0)
elif direc == 2:
#print("COME HERE2")
easyGo.mvRotate(ROTATE_SPEED, -1, False)
elif direc == 3:
easyGo.mvRotate(ROTATE_SPEED, -1, True)
def load_drive_component():
#Start driving procedure
print('Start driving componet...')
# load slam component
print('Slam loading')
slam_thread = threading.Thread(target=slam_launcher)
slam_thread.start()
# slam launch -> wait??
#auto drive
print('Autodirve loading')
autodrive_thread = threading.Thread(target=auto_drive_launcher)
autodrive_thread.start()
print('[INFO] Autodirve loaded')
#morp ->
print('[INFO] morp loading')
morp_thread = threading.Thread(target=morp_launcher)
morp_thread.start()
print('[INFO] morp loaded')
# [upper] localization and global moving
print('== ALL Component loaded ==')
handle_master('autodrive')
handle_tranfer = threading.Thread(target=morp_dap_transfer)
handle_tranfer.start()
if __name__ == "__main__":
load_drive_component()
pass
else:
print("ERROR!, run standalone")
raise("incompatible running type")