fix

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()

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)
 

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__':