修改程序中断

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -36,11 +36,31 @@ static systime getFrontTime(const vector<systime> time_seq, const vector<float>
 void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) {
     if (target_box.rect == cv::Rect2d()) return;
     // 判断是否发生装甲目标切换。
+    
+    // 如果是首帧追踪，直接记录不进行推算
+    if (last_box.rect == cv::Rect2d()) {
+        time_seq.emplace_back(frame_time);
+        double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+        double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+        angle_seq.emplace_back(yaw);
+        sendBoxPosition(0);
+        return;
+    }
+
     // 记录切换前一段时间目标装甲的角度和时间
     // 通过线性拟合计算出角度为0时对应的时间点
     // 通过两次装甲角度为零的时间差计算陀螺旋转周期
     // 根据旋转周期计算下一次装甲出现在角度为零的时间点
     if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
+        if (time_seq.size() < 2) {
+            // 采点不足以拟合直线
+            time_seq.clear();
+            angle_seq.clear();
+            last_front_time = frame_time;
+            sendBoxPosition(0);
+            anti_top_cnt++;
+            return;
+        }
         auto front_time = getFrontTime(time_seq, angle_seq);
         auto once_periodms = getTimeIntervalms(front_time, last_front_time);
 //        if (abs(once_periodms - top_periodms[-1]) > 50) {