新版反陀螺。

armor/src/armor_finder/find/find_armor_box.cpp

@@ -147,7 +147,14 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) {
                 armor_box.id = c;
             }
         }, armor_boxes.size());
-        sort(armor_boxes.begin(), armor_boxes.end());
+        sort(armor_boxes.begin(), armor_boxes.end(), [&](const ArmorBox &a, const ArmorBox &b){
+            if(last_box.rect != cv::Rect2d()){
+                return getPointLength(a.getCenter()-last_box.getCenter()) <
+                       getPointLength(b.getCenter()-last_box.getCenter());
+            }else{
+                return a < b;
+            }
+        });
         if(armor_boxes[0].id != 0){
             box = armor_boxes[0];
         }