修改程序中断

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

others/include/additions.h

@@ -35,6 +35,8 @@ void extract(cv::Mat &src);
 
 double getPointLength(const cv::Point2f &p);
 
+#include <mutex>
+
 // 循环队列
 template<class type, int length>
 class RoundQueue {
@@ -43,6 +45,7 @@ private:
     int head;
     int tail;
     int count;
+    mutable std::mutex mtx;
 public:
     RoundQueue<type, length>() : head(0), tail(0), count(0) {};
 
@@ -51,14 +54,17 @@ public:
     };
 
     int size() const {
+        std::lock_guard<std::mutex> lock(mtx);
         return count;
     };
 
     bool empty() const {
+        std::lock_guard<std::mutex> lock(mtx);
         return count == 0;
     };
 
     void push(const type &obj) {
+        std::lock_guard<std::mutex> lock(mtx);
         data[head] = obj;
         head = (head + 1) % length;
         if (count < length) {
@@ -69,15 +75,17 @@ public:
     };
 
     bool pop(type &obj) {
-        if (empty()) return false;
+        std::lock_guard<std::mutex> lock(mtx);
+        if (count == 0) return false;
         obj = data[tail];
         tail = (tail + 1) % length;
         count--;
         return true;
     };
 
-    type &operator[](int idx) {
-        if (count == 0) return data[0];
+    type operator[](int idx) const {
+        std::lock_guard<std::mutex> lock(mtx);
+        if (count == 0) return data[0]; // will return garbage but safe from crash
         if (idx < 0) idx += count;
         if (idx < 0) idx = 0;
         if (idx >= count) idx = count - 1;