diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 1250ebc..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,256 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include - -#include "config.h" -#include "MindVisionCamera.h" // 使用MindVision相机 -#include "ImagePreprocessor.h" -#include "ArmorDetector.h" -#include "KalmanFilter.h" -#include "Visualizer.h" -#include "BallisticPredictor.h" -#include "TTLCommunicator.h" - -// Function to output control data to TTL device (with enable control) -void output_control_data(const cv::Point2f* ballistic_point, - const std::string& target_color, - int frame_counter, - TTLCommunicator* ttl_communicator, - const cv::Point2f& img_center, - bool use_ttl) { - // Only send data when TTL is enabled, meets frame interval, and has valid target - if (use_ttl && frame_counter % 5 == 0 && ballistic_point != nullptr) { - std::string send_str; - - // Calculate offset (based on actual image center) - int ballistic_offset_x = static_cast(ballistic_point->x - img_center.x); - int ballistic_offset_y = static_cast(img_center.y - ballistic_point->y); - - // Color simplification mapping - std::string simplified_color = target_color; - if (target_color == "red") simplified_color = "r"; - else if (target_color == "blue") simplified_color = "b"; - - // Construct send string - send_str = "s " + simplified_color + " " + std::to_string(ballistic_offset_x) + " " + std::to_string(ballistic_offset_y); - - // Send data - if (ttl_communicator != nullptr) { - ttl_communicator->send_data(send_str); - } - } -} - -void set_camera_resolution(MindVisionCamera& , int width, int height) { - // The resolution is set during camera initialization in MindVision - // We need to implement a method in MindVisionCamera to change resolution - // For now, we'll just log the intended change - std::cout << "Setting camera resolution to: " << width << "x" << height << std::endl; -} - -int main(int /*argc*/, char* /*argv*/[]) { - std::string target_color = "red"; - int cam_id = 0; - cv::Size default_resolution(640, 480); - bool use_ttl = true; // Set to false to disable TTL communication - - // Define optional resolution list (adjust based on camera support) - std::vector resolutions = { - cv::Size(320, 240), // Low resolution, high frame rate - cv::Size(640, 480), // Standard resolution - cv::Size(1280, 720), // HD resolution - cv::Size(1920, 1080) // Full HD resolution - }; - - // Find the index of default resolution - int res_index = 1; // Default to index 1 (640x480) - for (size_t i = 0; i < resolutions.size(); i++) { - if (resolutions[i] == default_resolution) { - res_index = static_cast(i); - break; - } - } - - // Initialize TTL communication (only when enabled) - TTLCommunicator* ttl = nullptr; - if (use_ttl) { - // On Linux, the port would typically be /dev/ttyUSB0, /dev/ttyACM0, etc. - ttl = new TTLCommunicator("/dev/ttyUSB0", TTL_BAUDRATE); - if (ttl != nullptr) { - if (!ttl->connect()) { - std::cout << "Warning: Cannot establish TTL connection, will continue running but not send data" << std::endl; - } - } else { - std::cout << "Error: Failed to create TTL communicator instance" << std::endl; - } - } else { - std::cout << "TTL communication disabled" << std::endl; - } - - // Initialize visual processing modules with MindVision camera - MindVisionCamera camera(cam_id, target_color); - if (!camera.is_opened) { - std::cerr << "Cannot open MindVision camera!" << std::endl; - return -1; - } - - // Initialize ballistic predictor (adjustable bullet speed, e.g., 16m/s) - BallisticPredictor ballistic_predictor(16.0f); - // Record target speed (obtained from Kalman filter) - cv::Point2f target_speed(0.0f, 0.0f); - - // Set initial resolution - set_camera_resolution(camera, resolutions[res_index].width, resolutions[res_index].height); - std::cout << "Initial camera resolution: " << resolutions[res_index].width << "x" << resolutions[res_index].height << std::endl; - - ImagePreprocessor preprocessor; - ArmorDetector detector; - KalmanFilter kalman_tracker; - Visualizer visualizer; - - int frame_counter = 0; // Counter to control output frequency - int max_consecutive_predicts = 20; // Maximum consecutive prediction times - int consecutive_predicts = 0; // Current consecutive prediction count - - cv::Mat frame; - try { - while (true) { - // 使用新的颜色过滤方法同时获取图像和原始掩码 - cv::Mat raw_mask; - if (!camera.read_frame_with_color_filter(frame, raw_mask, target_color)) { - std::cout << "Cannot read from MindVision camera, exiting!，HERERER" << std::endl; - break; - } - - // Get actual image size and calculate center - cv::Point2f img_center(frame.cols / 2.0f, frame.rows / 2.0f); - - // 使用OpenCV进行形态学处理 - cv::Mat mask; - preprocessor.apply_morphology(raw_mask, mask); - - // 生成彩色图像（仅显示目标颜色） - cv::Mat color_only_frame; - frame.copyTo(color_only_frame, raw_mask); - - // Armor plate detection - std::vector valid_light_bars; - std::vector armor_plates; - detector.detect(mask, target_color, valid_light_bars, armor_plates); - - // Kalman filter tracking (modified part: get target speed) - cv::Point2f* current_center = nullptr; - if (!armor_plates.empty()) { - current_center = &(armor_plates[0].center); - } - - // Use smart pointer for safer memory management - std::unique_ptr predicted_center = nullptr; - - if (current_center != nullptr) { - // Has detection result: update Kalman filter, reset consecutive prediction count - kalman_tracker.update(*current_center); - predicted_center = std::make_unique(kalman_tracker.predict()); - - // Get velocity from the Kalman filter - target_speed = cv::Point2f(0.0f, 0.0f); // Kalman filter in OpenCV doesn't directly expose velocity - consecutive_predicts = 0; - } else { - // No detection result: only use Kalman prediction, limit consecutive predictions - consecutive_predicts++; - if (consecutive_predicts < max_consecutive_predicts) { - cv::Point2f temp_pred = kalman_tracker.predict(); - if (temp_pred.x != 0 || temp_pred.y != 0) { // Check if prediction is valid - predicted_center = std::make_unique(temp_pred); - } - } else { - predicted_center = nullptr; - target_speed = cv::Point2f(0.0f, 0.0f); - } - } - - // Determine display center - cv::Point2f* display_center = current_center; - if (display_center == nullptr && predicted_center != nullptr) { - display_center = predicted_center.get(); - } - bool is_predicted = (display_center != nullptr) && (current_center == nullptr); - - // Calculate ballistic prediction point - cv::Point2f* ballistic_point = ballistic_predictor.predict_ballistic_point( - predicted_center.get(), img_center, target_speed - ); - - auto current_time = std::chrono::high_resolution_clock::now(); - - // Visualization - visualizer.draw_light_bars(frame, valid_light_bars, target_color); - if (!armor_plates.empty()) { - visualizer.draw_armor_plate(frame, armor_plates[0]); - } - visualizer.draw_offset_text(frame, display_center, target_color, is_predicted); - visualizer.draw_ballistic_point(frame, ballistic_point); - - // Output control data to TTL (passing use_ttl to control whether to send) - output_control_data(display_center, target_color, frame_counter, ttl, img_center, use_ttl); - - // Display windows - cv::imshow("Armor Detection", frame); - cv::imshow(target_color + " Mask", mask); - cv::imshow(target_color + " Only", color_only_frame); - - // Exit on 'q' key press - if (cv::waitKey(1) == 'q') { - break; - } - - frame_counter++; - - // Control max frame rate (100 FPS) - auto end_time = std::chrono::high_resolution_clock::now(); - auto elapsed = std::chrono::duration_cast(end_time - current_time).count(); - if (elapsed < 10) { // 100 FPS = 10ms per frame - std::this_thread::sleep_for(std::chrono::milliseconds(10 - elapsed)); - } - - // Smart pointers automatically handle memory cleanup - } - } - catch (const std::exception& e) { - std::cerr << "Error in main loop: " << e.what() << std::endl; - } - catch (...) { - std::cerr << "Unknown error occurred in main loop" << std::endl; - } - - // Cleanup - try { - camera.release(); - } catch (...) { - std::cerr << "Error during camera release" << std::endl; - } - - try { - cv::destroyAllWindows(); - } catch (...) { - std::cerr << "Error during window destruction" << std::endl; - } - - // Only close TTL connection when enabled and initialized - if (use_ttl && ttl != nullptr) { - try { - ttl->close(); - } catch (...) { - std::cerr << "Error during TTL close" << std::endl; - } - delete ttl; - ttl = nullptr; - } - - return 0; -} \ No newline at end of file