From 6b13f0337c34a974cc10bcda89330be0a3e0bc75 Mon Sep 17 00:00:00 2001 From: raiot Date: Mon, 24 Jun 2024 00:23:34 +0800 Subject: [PATCH] fix --- agent.py | 7 +++++-- control.py | 11 +++++++++-- utils.py | 16 ++++++++++++---- 3 files changed, 26 insertions(+), 8 deletions(-) diff --git a/agent.py b/agent.py index e0de1b2..55b59f4 100644 --- a/agent.py +++ b/agent.py @@ -94,6 +94,7 @@ class UAV: self.update() def video_stream(self): + # 独立线程，用于获取视频流并记录状态 height, width, _ = self.frame_read.frame.shape while True: frame = self.frame_read.frame @@ -101,6 +102,9 @@ class UAV: states = self.tello.get_current_state() states['timestamp'] = time.time() + states['status'] = self.status + states['cmd_vel'] = [self.left_right_velocity, self.for_back_velocity, self.up_down_velocity, self.yaw_velocity] + self.recorder.state_record(states) self.recorder.frame_record(frame, 'origin') @@ -131,11 +135,10 @@ class UAV: elif self.status == Status.LANDING: bottom_img = self.frame_queue.get() self.auto_control.landing(bottom_img=bottom_img) + time.sleep(1 / 30) if __name__ == '__main__': uav = UAV() - # while True: - # print(uav.tello.get_current_state()) uav.run() \ No newline at end of file diff --git a/control.py b/control.py index ee28302..a01f1da 100644 --- a/control.py +++ b/control.py @@ -123,8 +123,10 @@ class AutoControl: _item = self.gTracker.track(front_img) if _item.getMessage(): self.loss_target_times = 0 - print("tracking") + logging.info('tracking') self.target = _item.getMessage()['target'] + print(self.target) + frame = _item.getFrame() cv2.imshow("track result", frame) self.uav.recorder.frame_record(frame, 'KCF_tracking') @@ -152,13 +154,15 @@ class AutoControl: self.uav.yaw_velocity = 0 self.uav.for_back_velocity = 0 - if self.target[1] > self.uav.front_cam_shape[1] * 0.9: + if self.target[1] > self.uav.front_cam_shape[1] * 0.8: print(self.target[1], self.uav.front_cam_shape[0]/10) logging.warn("ready to land") self.uav.yaw_velocity = 0 self.uav.for_back_velocity = 10 self.uav.status = Status.LANDING self.uav.tello.set_video_direction(Tello.CAMERA_DOWNWARD) + cv2.destroyAllWindows() + time.sleep(1) # 等待1秒，确保摄像头回传到位 def landing(self, bottom_img): tag_large = self.tag_detector.detect(bottom_img[:, :, 0], estimate_tag_pose=True, camera_params=([353.836278849487, 353.077028029136, 163.745870517989, 115.130883974855]), tag_size=0.169) @@ -172,9 +176,12 @@ class AutoControl: tag_list.append(tag) if tag_list: + # 如果检测到标签 self.get_tag_times += 1 if self.get_tag_times == 5: + # 检测到5次后才认为大部分tag已经进入视野 self.is_get_tag = True + for_vel = -1 * self.for_back_controler.control(tag_list[0].pose_t[0], time.time()) self.uav.for_back_velocity = int(for_vel) diff --git a/utils.py b/utils.py index eda8a5d..5b550cc 100644 --- a/utils.py +++ b/utils.py @@ -23,9 +23,9 @@ class DataRecorder: self.video_folder = self.root_folder / 'video' self.video_folder.mkdir(parents=True, exist_ok=False) - 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']) - self.writer.writeheader() + # 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']) + # self.writer.writeheader() else: logging.warn("实验数据将不被记录！") @@ -47,9 +47,17 @@ class DataRecorder: def state_record(self, states): if self.need_record: 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']) + 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.writerow(states) + def anything_record(self,data, data_name:str, header:list): + if self.need_record: + with open(str(self.root_folder) + '/' + data_name + '.csv', mode='a', newline='') as file: + writer = csv.DictWriter(file, fieldnames=header) + writer.writeheader() + writer.writerow(data) + if __name__ == '__main__':