energy changed

energy/include/energy/energy.h

@@ -25,7 +25,10 @@ class Energy {
 public:
 	Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色
 	~Energy();//默认析构函数
-	int run(cv::Mat &gimble_src, cv::Mat &base_src);
+	int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src);
+    int runBig(cv::Mat &gimble_src);
+    int runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src);
+    int runSmall(cv::Mat &gimble_src);
 	Serial &serial;//串口
 	void setEnergyRotationInit();//判断顺逆时针函数
 	void extract(cv::Mat &src);//框取图像中的一块区域进行处理
@@ -99,6 +102,7 @@ private:
 
     void rotate();//获取预测点位
 	void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差
+    double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离
 
 	void writeDownMark();//记录操作手标定的云台初始角度
 

energy/src/energy/param_init.cpp

@@ -26,7 +26,7 @@ void Energy::initEnergy() {
     target_polar_angle = -1000;
     last_target_polar_angle = -1000;
 	radius = 0;
-    energy_rotation_direction = CLOCKWISE;
+    energy_rotation_direction = ANTICLOCKWISE;
 	attack_distance = ATTACK_DISTANCE;
 	last_fans_cnt = 0;
 	last_armors_cnt = 0;

energy/src/energy/run.cpp

@@ -12,12 +12,23 @@ using std::vector;
 
 
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数为能量机关模式主控制流函数
+// 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
 // ---------------------------------------------------------------------------------------------------------------------
-int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
+int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src){
+    if(chassis_src.empty())runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
+    else {
+        runBig(chassis_src);
+        return 0;
+    }
+}
 
-    cv::Mat src = gimble_src;
 
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
+// ---------------------------------------------------------------------------------------------------------------------
+int Energy::runBig(cv::Mat &gimble_src){
 //    imshow("src",src);
     fans.clear();
     armors.clear();
@@ -30,30 +41,24 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
 //    imagePreprocess(src);
 //    imshow("img_preprocess",src);
 
-    threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
+    threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
 //    imshow("bin",src);
 
-    fans_cnt = findFan(src, last_fans_cnt);
+    fans_cnt = findFan(gimble_src, last_fans_cnt);
 //    cout<<"fans_cnt: "<<fans_cnt<<endl;
     if(fans_cnt==-1) return 0;//滤去漏判的帧
 //    if(fans_cnt>0)showFanContours("fan",src);
 //    fans_cnt=0;
 
-    armors_cnt = findArmor(src, last_armors_cnt);
+    armors_cnt = findArmor(gimble_src, last_armors_cnt);
 //    cout<<"armors_cnt: "<<armors_cnt<<endl;
     if(armors_cnt==-1) return 0;//滤去漏判的帧
 //    if(armors_cnt>0) showArmorContours("armor",src);
 
-    if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src);
+    if(armors_cnt != fans_cnt+1) return 0;
 
-    centerRs_cnt = findCenterR(src);
+    centerRs_cnt = findCenterR(gimble_src);
-    if(centerRs_cnt>0)showCenterRContours("R",src);
+//    if(centerRs_cnt>0)showCenterRContours("R", gimble_src);
-
-
-    if(armors_cnt != fans_cnt+1)
-    {
-        return 0;
-    }
 
     getAllArmorCenters();
     circleLeastFit();
@@ -63,6 +68,8 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
     getArmorPolarAngle();
     findTarget();
 
+    if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
+
     if (energy_rotation_init) {
         initRotation();
         return 0;
@@ -79,3 +86,26 @@ int Energy::run(cv::Mat &gimble_src, cv::Mat &base_src){
 
 
 
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为小能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
+// ---------------------------------------------------------------------------------------------------------------------
+int Energy::runSmall(cv::Mat &gimble_src, cv::Mat &chassis_src){
+    if(chassis_src.empty())runSmall(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
+    else return 0;
+}
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数为小能量机关模式主控制流函数，且步兵仅拥有云台摄像头
+// ---------------------------------------------------------------------------------------------------------------------
+int Energy::runSmall(cv::Mat &gimble_src){
+
+}
+
+
+
+

energy/src/energy/show/show.cpp

@@ -8,6 +8,8 @@ using std::cout;
 using std::endl;
 using std::vector;
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有扇叶
 // ---------------------------------------------------------------------------------------------------------------------
@@ -23,19 +25,18 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
     {
         image2show = src.clone();
     }
-	//cvtColor(image2show, image2show, COLOR_GRAY2RGB);
 	for (const auto &fan : fans)
 	{
 		Point2f vertices[4];      //定义矩形的4个顶点
 		fan.rect.points(vertices);   //计算矩形的4个顶点
 		for (int i = 0; i < 4; i++)
 			line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 2);
-		//cout << fan.rect.center << '\t' << fan.rect.angle << '\t';
-		//cout << endl;
 	}
 	imshow(windows_name, image2show);
 }
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有装甲板
 // ---------------------------------------------------------------------------------------------------------------------
@@ -51,19 +52,21 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat src) {
     {
         image2show = src.clone();
     }
-	//cvtColor(image2show, image2show, COLOR_GRAY2RGB);
 	for (const auto &armor : armors)
 	{
 		Point2f vertices[4];      //定义矩形的4个顶点
 		armor.rect.points(vertices);   //计算矩形的4个顶点
 		for (int i = 0; i < 4; i++)
 			line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2);
-		//cout << armor.rect.center << '\t' << armor.rect.angle << '\t';
-		//cout << endl;
 	}
 	imshow(windows_name, image2show);
 }
 
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于显示图像中所有扇叶和装甲板,并框出待击打装甲板
+// ---------------------------------------------------------------------------------------------------------------------
 void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
     if (src.empty())return;
     static Mat image2show;
@@ -75,25 +78,23 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
     {
         image2show = src.clone();
     }
-    //cvtColor(image2show, image2show, COLOR_GRAY2RGB);
     for (const auto &fan : fans)
     {
         Point2f vertices[4];      //定义矩形的4个顶点
         fan.rect.points(vertices);   //计算矩形的4个顶点
         for (int i = 0; i < 4; i++)
             line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4);
-//		cout << "fan" << fan.rect.size.height <<'\t'<< fan.rect.size.width << '\t' << '\t';
-//		cout << endl;
     }
     for (const auto &armor : armors)
     {
         Point2f vertices[4];      //定义矩形的4个顶点
         armor.rect.points(vertices);   //计算矩形的4个顶点
-        for (int i = 0; i < 4; i++)
+        for (int i = 0; i < 4; i++){
+            if(pointDistance(static_cast<cv::Point2f>(armor.rect.center),target_point) < 5)
+                line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4);
+            else
                 line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4);
-//		cout << "armor center: "<< armor.rect.center << '\t'<< "armor angle: "<< armor.rect.angle;
+        }
-//		cout << endl;
-
         cv::Point2f point = armor.rect.center;
         cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径
 
@@ -102,6 +103,8 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
     imshow(windows_name, image2show);
 }
 
+
+
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于显示图像中所有可能的风车中心候选区R
 // ---------------------------------------------------------------------------------------------------------------------

energy/src/energy/tool/tool.cpp

@@ -59,3 +59,15 @@ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){
 	point_2.x = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0);
 	point_2.y = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0);
 }
+
+
+//----------------------------------------------------------------------------------------------------------------------
+// 此函数用于计算两点距离
+// ---------------------------------------------------------------------------------------------------------------------
+
+double Energy::pointDistance(cv::Point point_1, cv::Point point_2){
+    double distance = 0;
+    distance = sqrt(pow(static_cast<double>(point_1.x - point_2.x),2)
+                    + pow(static_cast<double>(point_1.y - point_2.y),2));
+    return distance;
+}

main.cpp

@@ -26,16 +26,23 @@ using namespace std;
 mcu_data mcuData = {
         0,
         0,
-        BIG_ENERGY_STATE,
+        ARMOR_STATE,
         0,
         1,
         ENEMY_RED,
 };
 
+WrapperHead *video_gimble = nullptr;
+WrapperHead *video_chassis = nullptr;
+
+Serial serial(115200);
+uint8_t last_state = mcuData.state;
+
+ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier);
+Energy energy(serial, mcuData.enemy_color);
+
 int main(int argc, char *argv[]) {
     process_options(argc, argv);
-    Serial serial(115200);
-	uint8_t last_state = mcuData.state;
     thread receive(uartReceive, &serial);
 
     int from_camera = 1;
@@ -45,170 +52,81 @@ int main(int argc, char *argv[]) {
     }
 
     while (true) {
-        VideoWriter armor_video_writer, energy_video_writer;
-        if (save_video) {
-            initVideoWriter(armor_video_writer, PROJECT_DIR"/armor_video/");
-            initVideoWriter(energy_video_writer, PROJECT_DIR"/energy_video/");
-        }
-
-        WrapperHead *video_armor=nullptr;
-        WrapperHead *video_energy=nullptr;
         if (from_camera) {
-            video_armor = new CameraWrapper(0, "armor");
+            video_gimble = new CameraWrapper(0, "armor");
-            video_energy = new CameraWrapper(1, "energy");
+            video_chassis = new CameraWrapper(1, "energy");
         } else {
-//            string armor_video, energy_video;
+            video_gimble = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi");
-//            lastVideo(armor_video, PROJECT_DIR"/armor_video/");
+            video_chassis = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi");
-//            video_armor = new VideoWrapper(armor_video);
-//            lastVideo(energy_video, PROJECT_DIR"/energy_video/");
-//            video_energy = new VideoWrapper(energy_video);
-            video_armor = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi");
-            video_energy = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi");
         }
-        if (video_armor->init()) {
+        if (video_gimble->init()) {
             LOGM("video_armor source initialization successfully.");
         } else {
             LOGW("video_armor source unavailable!");
-            delete video_armor;
+            delete video_gimble;
-            video_armor = nullptr;
+            video_gimble = nullptr;
         }
-        if (video_energy->init()) {
+        if (video_chassis->init()) {
             LOGM("video_energy source initialization successfully.");
         } else {
             LOGW("video_energy source unavailable!");
-            delete video_energy;
+            delete video_chassis;
-            video_energy = nullptr;
+            video_chassis = nullptr;
         }
 
-        Mat energy_src, armor_src;
+        Mat gimble_src, chassis_src;
         for (int i = 0; i < 10; i++) {
-            if (video_armor) {
+            if (video_gimble) {
-                video_armor->read(armor_src);
+                video_gimble->read(gimble_src);
             }
-            if (video_energy) {
+            if (video_chassis) {
-                video_energy->read(energy_src);
+                video_chassis->read(chassis_src);
             }
         }
-
-        ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier);
-
-        Energy energy(serial, mcuData.enemy_color);
-
         bool ok = true;
         cout<<"start running"<<endl;
         do {
             CNT_TIME("Total", {
-                if (mcuData.state == BIG_ENERGY_STATE) {
+                if (mcuData.state != ARMOR_STATE) {//能量机关模式
-					if (last_state != BIG_ENERGY_STATE) {
+                    ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头
+                    if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
+                    if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
+
+                    if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
+//                        if (from_camera == 0) {
+//                            cv::resize(energy_src, energy_src, cv::Size(640, 480), 2);
+//                            imshow("resize", energy_src);
+//                            energy.extract(energy_src);
+//                        }
+                        if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
                             energy.setEnergyRotationInit();
                             cout << "set" << endl;
                         }
-					last_state = mcuData.state;
+                        energy.runBig(gimble_src, chassis_src);//击打大符
-                    if (video_armor && video_energy) {
-                        ok = video_armor->read(armor_src) && video_energy->read(energy_src);
-                        if (!ok) {
-                            delete video_armor;
-                            delete video_energy;
-                            video_armor = nullptr;
-                            video_energy = nullptr;
                     }
-                        if(save_video){
-                            Mat energy_save = energy_src.clone();
-                            cvtColor(energy_save,energy_save,COLOR_GRAY2BGR);
-                            armor_video_writer.write(armor_src);
-                            energy_video_writer.write(energy_save);
-                        }
-                        if (show_origin) {
-                            imshow("armor src", armor_src);
-                            imshow("energy src", energy_src);
-                        }
-//                        if (from_camera == 0) {
-//							cv::resize(energy_src, energy_src, cv::Size(640, 480), 2);
-//							imshow("resize", energy_src);
-//                            energy.extract(energy_src);
-//                        }
-                        energy.run(armor_src, energy_src);
-                        waitKey(1);
-                    }
-                    else {
-                        video_energy = new CameraWrapper(1, "energy");
-                        if(!video_energy->init()){
-                            delete video_energy;
-                            video_energy = nullptr;
-                        }
-                    }
-                }
-
                     else if (mcuData.state == SMALL_ENERGY_STATE) {
+                        energy.runSmall(gimble_src, chassis_src);//击打小符
+                    }
+                    last_state = mcuData.state;//更新上一帧状态
+                }
+                else if (mcuData.state == ARMOR_STATE) {//自瞄模式
                     last_state = mcuData.state;
-                    if (video_armor && video_energy) {
+                    ok = checkReconnect(video_gimble->read(gimble_src));
-                        ok = video_armor->read(armor_src) && video_energy->read(energy_src);
+                    if (save_video) saveVideos(gimble_src);
-                        if (!ok) {
+                    if (show_origin) showOrigin(gimble_src);
-                            delete video_armor;
-                            delete video_energy;
-                            video_armor = nullptr;
-                            video_energy = nullptr;
-                        }
-                        if(save_video){
-                            Mat energy_save = energy_src.clone();
-                            cvtColor(energy_save,energy_save,COLOR_GRAY2BGR);
-                            armor_video_writer.write(armor_src);
-                            energy_video_writer.write(energy_save);
-                        }
-                        if (show_origin) {
-                            imshow("armor src", armor_src);
-                            imshow("energy src", energy_src);
-                        }
-//                        if (from_camera == 0) {
-//							cv::resize(energy_src, energy_src, cv::Size(640, 480), 2);
-//							imshow("resize", energy_src);
-//                            energy.extract(energy_src);
-//                        }
-                        energy.run(armor_src, energy_src);
-                        waitKey(1);
-                    }
-                    else {
-                        video_energy = new CameraWrapper(1, "energy");
-                        if(!video_energy->init()){
-                            delete video_energy;
-                            video_energy = nullptr;
-                        }
-                    }
-                }
-
-                else if (mcuData.state == ARMOR_STATE) {
-                    last_state = mcuData.state;
-                    if (video_armor) {
-                        ok = video_armor->read(armor_src);
-                        if (!ok) {
-                            delete video_armor;
-                            video_armor = nullptr;
-                        }
-                        if(save_video){
-                            armor_video_writer.write(armor_src);
-                        }
-//                        flip(armor_src, armor_src, 0);
-                        if (show_origin) {
-                            imshow("armor src", armor_src);
-                        }
                     CNT_TIME("Armor Time", {
-                                armorFinder.run(armor_src);
+                            armorFinder.run(gimble_src);
                     });
-                    }
-                    else {
-                        video_armor = new CameraWrapper(0, "armor");
-                        if(!video_armor->init()){
-                            delete video_armor;
-                            video_armor = nullptr;
-                        }
-                    }
-                }
 
+                }
+                cv::waitKey(1);
             });
         } while (ok);
 
-        delete video_armor;
+        delete video_gimble;
-        delete video_energy;
+        video_gimble = nullptr;
+        delete video_chassis;
+        video_chassis = nullptr;
         cout << "Program fails. Restarting" << endl;
     }
     return 0;

others/include/additions/additions.h

@@ -29,7 +29,11 @@ struct mcu_data{
 extern mcu_data mcuData;
 
 void uartReceive(Serial *pSerial);
-void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix);
+bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect);
-void lastVideo(std::string &video_name, const std::string &filename_prefix);
+bool checkReconnect(bool is_gimble_connect);
+void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src);
+void saveVideos(const cv::Mat &gimble_src);
+void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src);
+void showOrigin(const cv::Mat &gimble_src);
 
 #endif /* _ADDITIONS_H_ */

others/include/camera/wrapper_head.h

@@ -15,7 +15,6 @@ public:
     virtual ~WrapperHead() = default;;
     virtual bool init() = 0;
     virtual bool read(cv::Mat &src) = 0;
-
 };
 
 

others/src/additions/additions.cpp

@@ -4,14 +4,30 @@
 
 #include <cstring>
 #include <fstream>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
 #include <additions/additions.h>
+#include <camera/camera_wrapper.h>
 #include <log.h>
 #include <opencv2/videoio/videoio_c.h>
 #include <iostream>
+#include <energy/energy.h>
+#include <armor_finder/armor_finder.h>
 
 #define RECEIVE_LOG_LEVEL LOG_MSG
 
 using namespace std;
+using namespace cv;
+
+extern WrapperHead *video_gimble;
+extern WrapperHead *video_chassis;
+
+extern Serial serial;
+extern uint8_t last_state;
+
+extern ArmorFinder armorFinder;
+extern Energy energy;
 
 void uartReceive(Serial* pSerial) {
     char buffer[20];
@@ -38,7 +54,8 @@ void uartReceive(Serial* pSerial) {
     }
 }
 
-void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix){
+cv::VideoWriter initVideoWriter(const std::string &filename_prefix){
+    cv::VideoWriter video;
     std::ifstream in(filename_prefix + "cnt.txt");
     int cnt = 0;
     if (in.is_open())
@@ -54,15 +71,85 @@ void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix)
         out.close();
     }
     video.open(file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true);
+    return video;
 }
 
-void lastVideo(std::string &video_name, const std::string &filename_prefix){
+bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect){
-    std::ifstream in(filename_prefix + "cnt.txt");
+    if(!is_gimble_connect){
-    int cnt = 0;
+        video_gimble = new CameraWrapper(0, "armor");
-    if (in.is_open())
+        if(!(is_gimble_connect = video_gimble->init())){
-    {
+            delete video_gimble;
-        in >> cnt;
+            video_gimble = nullptr;
-        in.close();
         }
-    if(cnt > 1) std::string video_name = filename_prefix + std::to_string(cnt) + ".avi";
+    }
+    if(!is_chassis_connect){
+        video_chassis = new CameraWrapper(1, "energy");
+        if(!(is_chassis_connect = video_chassis->init())){
+            delete video_chassis;
+            video_chassis = nullptr;
+        }
+    }
+    return is_gimble_connect && is_chassis_connect;
+}
+
+bool checkReconnect(bool is_gimble_connect){
+    if(!is_gimble_connect){
+        video_gimble = new CameraWrapper(0, "armor");
+        if(!(is_gimble_connect = video_gimble->init())){
+            delete video_gimble;
+            video_gimble = nullptr;
+        }
+    }
+    return is_gimble_connect;
+}
+
+auto gimble_video_writer = initVideoWriter(PROJECT_DIR"/gimble_video/");
+auto chassis_video_writer = initVideoWriter(PROJECT_DIR"/chassis_video/");
+
+void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src){
+    if(!gimble_src.empty() && !chassis_src.empty()){
+        gimble_video_writer.write(gimble_src);
+        Mat chassis_save = chassis_src.clone();
+        cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR);
+        chassis_video_writer.write(chassis_save);
+    }
+    else if(!gimble_src.empty() && chassis_src.empty()){
+        gimble_video_writer.write(gimble_src);
+    }
+    else if(gimble_src.empty() && !chassis_src.empty()){
+        Mat chassis_save = chassis_src.clone();
+        cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR);
+        chassis_video_writer.write(chassis_save);
+    }
+    else return;
+}
+
+void saveVideos(const cv::Mat &gimble_src){
+    if(!gimble_src.empty()){
+        gimble_video_writer.write(gimble_src);
+    }
+    else return;
+}
+
+void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src){
+    if(!gimble_src.empty() && !chassis_src.empty()){
+        imshow("gimble", gimble_src);
+        imshow("chassis", chassis_src);
+    }
+    else if(!gimble_src.empty() && chassis_src.empty()){
+        imshow("gimble", gimble_src);
+    }
+    else if(gimble_src.empty() && !chassis_src.empty()){
+        imshow("chassis", chassis_src);
+    }
+    else return;
+    cv::waitKey(1);
+}
+
+void showOrigin(const cv::Mat &gimble_src){
+    if(!gimble_src.empty()){
+        imshow("gimble", gimble_src);
+    }
+    else return;
+    cv::waitKey(1);
 }

others/src/camera/camera_wrapper.cpp

@@ -136,6 +136,7 @@ bool CameraWrapper::readRaw(cv::Mat &src) {
 
         return true;
     } else {
+        src = cv::Mat();
         return false;
     }
 }
@@ -155,6 +156,7 @@ bool CameraWrapper::readProcessed(cv::Mat &src) {
         CameraReleaseImageBuffer(h_camera, pby_buffer);
         return true;
     } else {
+        src = cv::Mat();
         return false;
     }
 }