From c72dca8e8e8d51bcc7ede723879771d21fbc0108 Mon Sep 17 00:00:00 2001 From: AiYangSky <1732570904@qq.com> Date: Wed, 6 Sep 2023 19:47:06 +0800 Subject: [PATCH] add gimbal test --- CMakeLists.txt | 3 + samples/test/gimbal_test.cpp | 178 +++++++++++++++++++++++++++++++++++ scripts/test/test_fps.sh | 2 + 3 files changed, 183 insertions(+) create mode 100644 samples/test/gimbal_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 44707a4..9c10571 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -275,6 +275,9 @@ target_link_libraries(GimbalUdpDetectionInfoSender sv_world) add_executable(EvalFpsOnVideo samples/test/eval_fps_on_video.cpp) target_link_libraries(EvalFpsOnVideo sv_world) +add_executable(GimbalTest samples/test/gimbal_test.cpp) +target_link_libraries(GimbalTest sv_world) + add_executable(EvalModelOnCocoVal samples/test/eval_mAP_on_coco_val/eval_mAP_on_coco_val.cpp) target_link_libraries(EvalModelOnCocoVal sv_world) diff --git a/samples/test/gimbal_test.cpp b/samples/test/gimbal_test.cpp new file mode 100644 index 0000000..0db3de6 --- /dev/null +++ b/samples/test/gimbal_test.cpp @@ -0,0 +1,178 @@ +#include +#include +// 包含SpireCV SDK头文件 +#include +#include + +// yaw roll pitch +double gimbalEulerAngle[3]; +bool revFlag = false; +void gimableCallback(double &frame_ang_r, double &frame_ang_p, double &frame_ang_y, + double &imu_ang_r, double &imu_ang_p, double &imu_ang_y, + double &fov_x, double &fov_y) +{ + revFlag = true; + gimbalEulerAngle[0] = imu_ang_r; + gimbalEulerAngle[1] = imu_ang_p; + gimbalEulerAngle[2] = frame_ang_y; +} + +int main(int argc, char *argv[]) +{ + std::cout << "start " << argv[0] << " ...." << std::endl; + + if (argc != 3) + { + std::cout << "param error" << std::endl; + return -1; + } + + sv::Gimbal gimbal(sv::GimbalType::G1, sv::GimbalLink::SERIAL); + + gimbal.setSerialPort(argv[1]); + if (!gimbal.open(gimableCallback)) + { + std::cout << "IO open error" << std::endl; + return -1; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + if (!revFlag) + { + std::cout << "IO error,without data.failed !!!!!" << std::endl; + return -1; + } + + std::cout << " start set home test " << std::endl; + gimbal.setHome(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + for (uint8_t i = 0; i < 2; i++) + { + if (fabs(gimbalEulerAngle[i]) > 0.1f) + { + std::cout << " gimbal set home error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + } + if (fabs(gimbalEulerAngle[2]) > 3.0f) + { + std::cout << " gimbal set angle error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + + std::cout << " pass... " << std::endl; + std::cout << " start set angle 1 test " << std::endl; + + double angleSet[3] = {30, 90, 45}; + gimbal.setAngleEuler(angleSet[0], angleSet[1], angleSet[2], 0, 0, 0); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + for (uint8_t i = 0; i < 3; i++) + { + if (fabs(gimbalEulerAngle[i] - angleSet[i]) > 1.0f) + { + std::cout << " gimbal set angle error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + } + + std::cout << " pass... " << std::endl; + std::cout << " start set angle 2 test " << std::endl; + + angleSet[0] = -angleSet[0]; + angleSet[1] = -30; + angleSet[2] = -angleSet[2]; + gimbal.setAngleEuler(angleSet[0], angleSet[1], angleSet[2], 0, 0, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + for (uint8_t i = 0; i < 3; i++) + { + if (fabs(gimbalEulerAngle[i] - angleSet[i]) > 1.0f) + { + std::cout << " gimbal set angle error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + } + + std::cout << " pass... " << std::endl; + std::cout << " start set angle rate test " << std::endl; + + gimbal.setHome(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + angleSet[0] = 20; + angleSet[1] = 20; + angleSet[2] = 20; + + for (uint8_t i = 0; i < 51; i++) + { + gimbal.setAngleRateEuler(angleSet[0], angleSet[1], angleSet[2]); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + + gimbal.setAngleRateEuler(0, 0, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + for (uint8_t i = 0; i < 3; i++) + { + if (fabs(gimbalEulerAngle[i] - angleSet[i]) > 0.7f) + { + std::cout << " gimbal set angle rate error , failed !!!!!" << std::endl; + std::cout << "YRP:" << gimbalEulerAngle[0] << std::endl + << gimbalEulerAngle[1] << std::endl + << gimbalEulerAngle[2] << std::endl; + return -1; + } + } + + gimbal.setHome(); + std::cout << " pass... " << std::endl; + std::cout << " start image test " << std::endl; + + sv::Camera cap; + cap.setIp(argv[2]); + cap.setWH(1280, 720); + cap.setFps(30); + + cap.open(sv::CameraType::G1); + + if (!cap.isRunning()) + { + std::cout << " gimbal image error , failed !!!!!" << std::endl; + return -1; + } + + cv::Mat img; + uint16_t count = 0; + + for (uint16_t i = 0; i < 300; i++) + { + if (cap.read(img)) + { + count++; + if (count > 10) + { + std::cout << " pass... " << std::endl; + return 0; + } + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + std::cout << " gimbal image error , failed !!!!!" << std::endl; + return -1; +} \ No newline at end of file diff --git a/scripts/test/test_fps.sh b/scripts/test/test_fps.sh index 1e34932..961c2e7 100755 --- a/scripts/test/test_fps.sh +++ b/scripts/test/test_fps.sh @@ -43,3 +43,5 @@ make -j ./EvalFpsOnVideo ${HOME}/SpireCV/test/ellipse_640x480.mp4 ${HOME}/SpireCV/calib_webcam_640x480.yaml 3 ./EvalFpsOnVideo ${HOME}/SpireCV/test/ellipse_1280x720.mp4 ${HOME}/SpireCV/calib_webcam_1280x720.yaml 3 +./GimbalTest /dev/ttyUSB0 192.168.2.64 +