-
Notifications
You must be signed in to change notification settings - Fork 0
/
task_3.py
147 lines (135 loc) · 5.78 KB
/
task_3.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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
from apis_task_3 import Controller
import vrep
import cv2
import numpy as np
from runnable.distance_metric import zedDistance
from speed_predict import SpeedPredictor
import copy
from runnable.distance_metric import get_people_pos
from people_choose import PeopleChoose
from speed_predict_task3 import SpeedPredictor
from face_recognition.face_tracking import resNet
from queue import Queue
def main():
# finish first
vrep.simxFinish(-1)
# connect to server
clientID = vrep.simxStart("127.0.0.1", 19997, True, True, 5000, 5)
if clientID != -1:
print("Connect Succesfully.")
else:
print("Connect failed.")
assert 0
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# get handles
_, vision_handle_0 = vrep.simxGetObjectHandle(clientID, "zed_vision0", vrep.simx_opmode_blocking)
_, vision_handle_1 = vrep.simxGetObjectHandle(clientID, "zed_vision1", vrep.simx_opmode_blocking)
_, controller_handle= vrep.simxGetObjectHandle(clientID, "Quadricopter_target", vrep.simx_opmode_blocking)
_, base_handle = vrep.simxGetObjectHandle(clientID, "Quadricopter", vrep.simx_opmode_blocking)
# set Controller
synchronous_flag = True
time_interval = 0.05
flight_controller = Controller(
clientID=clientID,
base_handle=base_handle,
controller_handle=controller_handle,
vision_handle_0=vision_handle_0,
vision_handle_1=vision_handle_1,
synchronous=synchronous_flag,
time_interval=time_interval,
v_max=0.0305,
v_add=0.0005,
v_sub=0.0005,
v_min=0.03,
v_constant=0.02,
use_constant_v=False,
)
# set controller position
base_position = flight_controller.getPosition('base')
flight_controller.setPosition('controller', base_position)
# start simulation
vrep.simxSynchronous(clientID, synchronous_flag)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
target_name = "Bill#2"
photo_num = 0
while True:
# 巡航搜索
search_points = [[-4.175, -0.2]]
photo_interval = 50
photo_count = photo_interval
find_flag = False
pos_now = np.array(flight_controller.getPosition('controller')[:2])
start_num = find_nearest_point_num(pos_now, search_points)
last_position = [4.65, -7.4]
while True:
for target_point_0 in search_points[start_num:]:
target_point = copy.deepcopy(target_point_0)
target_point.append(base_position[2])
target_point = np.array(target_point)
flight_controller.moveTo(target_point, 1, 1, True)
while flight_controller.left_time > 0:
if photo_count >= photo_interval:
flight_controller.to_take_photos()
photo_count = 0
else:
photo_count += 1
result = flight_controller.step_forward_move()
if 'photos' in result:
cv2.imwrite("task_3/"+str(photo_num)+"zed0.jpg", result['photos'][0])
cv2.imwrite("task_3/"+str(photo_num)+"zed1.jpg", result['photos'][1])
pos = resNet(result['photos'][1], result['photos'][0], 4, clientID)
print(pos)
if pos is not None:
target_position = pos
find_flag = True
break
photo_num += 1
if find_flag:
break
if find_flag:
break
start_num = 0
if find_flag:
break
print("Find target", target_position)
flight_controller.moving_queue = Queue()
# _, target_handle = vrep.simxGetObjectHandle(clientID, target_name, vrep.simx_opmode_blocking)
# _, target_position = vrep.simxGetObjectPosition(clientID, target_handle, -1, vrep.simx_opmode_blocking)
target_position[2] = base_position[2]
target_position = np.array(target_position)
time_interval = 5
people_chooser = PeopleChoose(pos_threshold=1, ori_threshold=0, color_threshold=None)
people_chooser.last_position = target_position[:2]
print("Target:", target_position)
while True:
print(photo_num)
move_to_position = copy.deepcopy(target_position)
move_to_position[1] += 3
print(target_position, move_to_position)
flight_controller.moveTo(np.array(move_to_position), 1, 1, True)
flight_controller.to_take_photos()
for i in range(time_interval):
result = flight_controller.step_forward_move()
if 'photos' in result:
cv2.imwrite("task_3_2/"+str(photo_num)+"zed0.jpg", result['photos'][0])
cv2.imwrite("task_3_2/"+str(photo_num)+"zed1.jpg", result['photos'][1])
photo_0 = cv2.imread("task_3_2/"+str(photo_num)+"zed0.jpg")
photo_1 = cv2.imread("task_3_2/"+str(photo_num)+"zed1.jpg")
pos_list = get_people_pos(clientID, photo_1, photo_0)
photo_num += 1
next_position = people_chooser.find_next_position(pos_list)
next_position.append(target_position[2])
target_position = np.array(next_position)
print("Target:", target_position)
def find_nearest_point_num(pos_now, search_points):
distance_min = -1
num_min = -1
for num in range(len(search_points)):
delta_distance = np.linalg.norm(search_points[num] - pos_now)
if delta_distance < distance_min or distance_min == -1:
distance_min = delta_distance
num_min = num
return num_min
if __name__ == '__main__':
main()