raiot
/
USVLander
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'main' of https://github.com/raiots/USVLander

This commit is contained in:
raiot 2024-06-29 19:22:46 +08:00
parent 68ff57d137 ba7f14f654
commit 318f710ef8
3 changed files with 31 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
agent.py
View File

@ -59,12 +59,12 @@ class UAV:
if self.send_rc_control: if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity,
self.up_down_velocity, self.yaw_velocity) self.up_down_velocity, self.yaw_velocity)
print(self.left_right_velocity, self.for_back_velocity, print(self.left_right_velocity, self.for_back_velocity,
self.up_down_velocity, self.yaw_velocity) self.up_down_velocity, self.yaw_velocity)
def run(self): def run(self):
""" 主程序 """ """ 主程序 """
# self.init_flight() self.init_flight()
# self.tello.turn_motor_on() # self.tello.turn_motor_on()
state_thread = Thread(target=self.video_stream) state_thread = Thread(target=self.video_stream)
@ -92,6 +92,7 @@ class UAV:
self.tello.takeoff() self.tello.takeoff()
self.send_rc_control = True # 开启发送控制信号 self.send_rc_control = True # 开启发送控制信号
self.update() self.update()
self.tello.go_xyz_speed(0, 0, 50, 20) # 初始升高高度
def video_stream(self): def video_stream(self):
# 独立线程，用于获取视频流并记录状态 # 独立线程，用于获取视频流并记录状态
@ -135,6 +136,10 @@ class UAV:
elif self.status == Status.LANDING: elif self.status == Status.LANDING:
bottom_img = self.frame_queue.get() bottom_img = self.frame_queue.get()
self.auto_control.landing(bottom_img=bottom_img) self.auto_control.landing(bottom_img=bottom_img)
elif self.status == Status.FALLING:
# 自由落体阶段
pass
time.sleep(1 / 30) time.sleep(1 / 30)

27
control.py
View File

@ -125,7 +125,9 @@ class AutoControl:
self.loss_target_times = 0 self.loss_target_times = 0
logging.info('tracking') logging.info('tracking')
self.target = _item.getMessage()['target'] self.target = _item.getMessage()['target']
print(self.target) # print(self.target)
target_dict = {'timestamp': time.time(), 'x': self.target[0], 'y':self.target[1]}
self.uav.recorder.anything_record(target_dict, 'track_result', ['timestamp', 'x', 'y'])
frame = _item.getFrame() frame = _item.getFrame()
cv2.imshow("track result", frame) cv2.imshow("track result", frame)
@ -178,21 +180,22 @@ class AutoControl:
if tag_list: if tag_list:
# 如果检测到标签 # 如果检测到标签
self.get_tag_times += 1 self.get_tag_times += 1
if self.get_tag_times == 5: if self.get_tag_times >= 5:
# 检测到5次后才认为大部分tag已经进入视野 # 检测到5次后才认为大部分tag已经进入视野
self.is_get_tag = True self.is_get_tag = True
for_vel = -1 * self.for_back_controler.control(tag_list[0].pose_t[0], time.time()) for_vel = -1 * self.for_back_controler.control(tag_list[0].pose_t[0], time.time())
self.uav.for_back_velocity = int(for_vel) self.uav.for_back_velocity = int(for_vel)
left_vel = -1 * self.left_right_controler.control(tag_list[0].pose_t[1], time.time()) left_vel = -1 * self.left_right_controler.control(tag_list[0].pose_t[1], time.time())
self.uav.left_right_velocity = int(left_vel) self.uav.left_right_velocity = int(left_vel)
# print(self.uav.for_back_velocity, self.uav.left_right_velocity) # print(self.uav.for_back_velocity, self.uav.left_right_velocity)
if tag_list[0].pose_t[0] < 0.2 and tag_list[0].pose_t[1] < 0.2: if tag_list[0].pose_t[0] < 0.2 and tag_list[0].pose_t[1] < 0.2:
self.uav.up_down_velocity = -20 self.uav.up_down_velocity = -20
# print(tag_list[0].pose_t) # 后正左正高度 # print(tag_list[0].pose_t) # 后正左正高度
if tag_list[0].pose_t[2] < 0.3: if tag_list[0].pose_t[2] < 0.3:
self.uav.tello.land() self.uav.tello.land()
self.uav.status = Status.FALLING
else: else:
self.uav.up_down_velocity = 0 self.uav.up_down_velocity = 0
self.uav.left_right_velocity = 0 self.uav.left_right_velocity = 0

13
utils.py
View File

@ -12,6 +12,7 @@ class Status(Enum):
DETECT = 1 DETECT = 1
TRACK = 2 TRACK = 2
LANDING = 3 LANDING = 3
FALLING = 4
class DataRecorder: class DataRecorder:
def __init__(self, exp_note='default', need_record=False): def __init__(self, exp_note='default', need_record=False):
@ -23,9 +24,9 @@ class DataRecorder:
self.video_folder = self.root_folder / 'video' self.video_folder = self.root_folder / 'video'
self.video_folder.mkdir(parents=True, exist_ok=False) self.video_folder.mkdir(parents=True, exist_ok=False)
# with open(str(self.root_folder) + '/sensor_data.csv', mode='a', newline='') as file: with open(str(self.root_folder) + '/sensor_data.csv', mode='a', newline='') as file:
# self.writer = csv.DictWriter(file, fieldnames=['timestamp', 'mid', 'x', 'y', 'z', 'mpry', 'pitch', 'roll', 'yaw', 'vgx', 'vgy', 'vgz', 'templ', 'temph', 'tof', 'h', 'bat', 'baro', 'time', 'agx', 'agy', 'agz']) writer = csv.DictWriter(file, fieldnames=['timestamp', 'mid', 'x', 'y', 'z', 'mpry', 'pitch', 'roll', 'yaw', 'vgx', 'vgy', 'vgz', 'templ', 'temph', 'tof', 'h', 'bat', 'baro', 'time', 'agx', 'agy', 'agz', 'status', 'cmd_vel'])
# self.writer.writeheader() writer.writeheader()
else: else:
logging.warn("实验数据将不被记录！") logging.warn("实验数据将不被记录！")
@ -48,14 +49,14 @@ class DataRecorder:
if self.need_record: if self.need_record:
with open(str(self.root_folder) + '/sensor_data.csv', mode='a', newline='') as file: with open(str(self.root_folder) + '/sensor_data.csv', mode='a', newline='') as file:
writer = csv.DictWriter(file, fieldnames=['timestamp', 'mid', 'x', 'y', 'z', 'mpry', 'pitch', 'roll', 'yaw', 'vgx', 'vgy', 'vgz', 'templ', 'temph', 'tof', 'h', 'bat', 'baro', 'time', 'agx', 'agy', 'agz', 'status', 'cmd_vel']) writer = csv.DictWriter(file, fieldnames=['timestamp', 'mid', 'x', 'y', 'z', 'mpry', 'pitch', 'roll', 'yaw', 'vgx', 'vgy', 'vgz', 'templ', 'temph', 'tof', 'h', 'bat', 'baro', 'time', 'agx', 'agy', 'agz', 'status', 'cmd_vel'])
writer.writeheader() # writer.writeheader()
writer.writerow(states) writer.writerow(states)
def anything_record(self,data, data_name:str, header:list): def anything_record(self, data, data_name:str, header:list):
if self.need_record: if self.need_record:
with open(str(self.root_folder) + '/' + data_name + '.csv', mode='a', newline='') as file: with open(str(self.root_folder) + '/' + data_name + '.csv', mode='a', newline='') as file:
writer = csv.DictWriter(file, fieldnames=header) writer = csv.DictWriter(file, fieldnames=header)
writer.writeheader() # writer.writeheader()
writer.writerow(data) writer.writerow(data)