API tools faq
paste
Advertisement
Guest User

Buk-S Automated Firing Solution

a guest
Dec 26th, 2014
248
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 11.30 KB | None | 0 0
raw download clone embed print report
  1. #include "stdafx.h"
  2. #include "hidapi.h"
  3. #include "opencv2/core/core.hpp"
  4. #include "opencv2/imgproc/imgproc.hpp"
  5. #include "opencv2/highgui/highgui.hpp"
  6. #include <algorithm>
  7. #include <fstream>
  8. #include <iostream>
  9. #include <stdio.h>
  10. #include <stdlib.h>
  11. #include <windows.h>
  12.  
  13. using namespace cv;
  14. using namespace std;
  15.  
  16. void drawRect(cv::Mat &background, int sx, int sy, int width, int height, int col_r, int col_g, int col_b);
  17. void obtainTargets(const cv::Mat &background, const std::vector<cv::Mat> &hostiles, const std::vector<cv::Mat> &hostiles_2, std::vector<std::vector<float > > &hmatch);
  18. void overlayImage(const cv::Mat &background, const cv::Mat &foreground, cv::Mat &output, cv::Point2i location);
  19.  
  20. int main(int argc, char* argv[]) {
  21.  
  22.     // HID API Initialization
  23.     int res;
  24.     unsigned char buf[65];
  25.     #define MAX_STR 255
  26.     wchar_t wstr[MAX_STR];
  27.     hid_device *handle;
  28.     int i;
  29.  
  30.     // Enumerate and print the HID devices on the system
  31.     bool found_launcher=false;
  32.     unsigned short launcher_vid, launcher_pid;
  33.     struct hid_device_info *devs, *cur_dev;
  34.    
  35.     devs = hid_enumerate(0x0, 0x0);
  36.     cur_dev = devs;
  37.     while (cur_dev) {
  38.         char cdps_c[21];
  39.         wcstombs(cdps_c,cur_dev->product_string,20); cdps_c[20] = '\0'; std::string cdps(cdps_c);
  40.         if (cdps.compare("USB Missile Launcher")==0) {
  41.             std::cout << "Opening ML" << std::endl;
  42.             launcher_vid=cur_dev->vendor_id;
  43.             launcher_pid=cur_dev->product_id;
  44.             found_launcher=true;
  45.         } else {
  46.             std::cout << "Name: " << cdps << std::endl;
  47.         }
  48.  
  49.         printf("Device Found\n  type: %04hx %04hx\n  path: %s\n  serial_number: %ls",
  50.             cur_dev->vendor_id, cur_dev->product_id, cur_dev->path, cur_dev->serial_number);
  51.         printf("\n");
  52.         printf("  Manufacturer: %ls\n", cur_dev->manufacturer_string);
  53.         printf("  Product:      %ls\n", cur_dev->product_string);
  54.         printf("\n");
  55.         cur_dev = cur_dev->next;
  56.     }
  57.     hid_free_enumeration(devs);
  58.  
  59.     // Connect to missile launcher
  60.     if (found_launcher) {
  61.         handle = hid_open(launcher_vid, launcher_pid, NULL);
  62.         std::cout << "handle: " << handle << std::endl;
  63.     } else {
  64.         std::cout << "Cannot connect to launcher." << endl;
  65.         exit(0);
  66.     }
  67.  
  68.     VideoCapture cap(0); // open the video camera no. 0
  69.  
  70.     if (!cap.isOpened()) {
  71.         std::cout << "Cannot open the video cam." << endl;
  72.         exit(0);
  73.     }
  74.  
  75.     double dWidth = cap.get(CV_CAP_PROP_FRAME_WIDTH); //get the width of frames of the video
  76.     double dHeight = cap.get(CV_CAP_PROP_FRAME_HEIGHT); //get the height of frames of the video
  77.  
  78.     std::cout << "Frame size : " << dWidth << " x " << dHeight << endl;
  79.  
  80.     // Load hostile targets
  81.     std::vector<Mat> h; h.resize(6);
  82.     h[0] = imread("h0.png", CV_LOAD_IMAGE_GRAYSCALE);
  83.     h[1] = imread("h1.png", CV_LOAD_IMAGE_GRAYSCALE);
  84.     h[2] = imread("h2.png", CV_LOAD_IMAGE_GRAYSCALE);
  85.     h[3] = imread("h3.png", CV_LOAD_IMAGE_GRAYSCALE);
  86.     h[4] = imread("h4.png", CV_LOAD_IMAGE_GRAYSCALE);
  87.     h[5] = imread("h5.png", CV_LOAD_IMAGE_GRAYSCALE);
  88.  
  89.     std::vector<Mat> h2; h2.resize(6);
  90.     resize(h[0], h2[0], Size(7,7), 0, 0, INTER_CUBIC);
  91.     resize(h[1], h2[1], Size(7,7), 0, 0, INTER_CUBIC);
  92.     resize(h[2], h2[2], Size(7,7), 0, 0, INTER_CUBIC);
  93.     resize(h[3], h2[3], Size(7,7), 0, 0, INTER_CUBIC);
  94.     resize(h[4], h2[4], Size(7,7), 0, 0, INTER_CUBIC);
  95.     resize(h[5], h2[5], Size(7,7), 0, 0, INTER_CUBIC);
  96.  
  97.     // Load videocam overlay
  98.     cout << "Loading overlay..." << endl;
  99.     Mat overlay = imread("overlay.png",-1);
  100.     resize(overlay, overlay, Size(640, 480), 0, 0, INTER_CUBIC);
  101.    
  102.     namedWindow("Buk-S Automated Firing Solution",CV_WINDOW_AUTOSIZE);
  103.     //namedWindow("MyVideo",CV_WINDOW_NORMAL);
  104.     //setWindowProperty("MyVideo", CV_WND_PROP_FULLSCREEN, 1.0);
  105.     // Add a wait to allow webcam to initialize reliably
  106.     unsigned int last_call_time=timeGetTime(); unsigned int now_time=timeGetTime(); while (now_time-last_call_time<2000) { now_time=timeGetTime(); }
  107.  
  108.     unsigned int last_cmd_time=timeGetTime();
  109.     unsigned int last_fire_time=timeGetTime()-5000;
  110.     bool frame_written=false;
  111.     while (1) {
  112.         Mat frame, gframe, oframe;
  113.  
  114.         bool bSuccess = cap.read(frame); // read a new frame from video
  115.         cv::Size s = frame.size();
  116.  
  117.         // N.B. don't break, first frame tends to fail
  118.         if (!bSuccess) {
  119.              std::cout << "Cannot read a frame from video stream" << endl;
  120.              //break;
  121.         } else if (!frame_written && s.width>0 && s.height>0) {
  122.             frame_written=true;
  123.             imwrite("frame.jpg", frame);
  124.         }
  125.  
  126.         if (s.width>0 && s.height>0) {
  127.             // Detect targets
  128.             cvtColor(frame,gframe,CV_BGR2GRAY);
  129.  
  130.             std::vector<std::vector<float > > hm; hm.resize(6); for (int i=0;i<6;i++) { hm[i].resize(3); for (int j=0;j<2;j++) { hm[i][j]=-9999.0f; } hm[i][2]=999.0f; }
  131.  
  132.             resize(gframe, gframe, Size(320, 240), 0, 0, INTER_CUBIC);
  133.             obtainTargets(gframe,h,h2,hm);
  134.  
  135.             int active_target=0;
  136.             for (int t=1;t<=5;t++) {
  137.                 if (hm[t][2]<3.0f && hm[t][0]>=0.0f && hm[t][1]>=0.0f) {
  138.                     drawRect(frame,hm[t][0]*2-8,hm[t][1]*2-8,42,42,0,255,0);
  139.                     active_target=t;
  140.                     break;
  141.                 }
  142.             }
  143.  
  144.             overlayImage(frame, overlay, oframe, cv::Point(0,0));
  145.             resize(oframe, oframe, Size(640, 480), 0, 0, INTER_CUBIC);
  146.             imshow("Buk-S Automated Firing Solution", oframe);
  147.  
  148.             // Move towards target
  149.             if (active_target>0 && timeGetTime()-last_cmd_time>500 && timeGetTime()-last_fire_time>5000) {
  150.                 if (hm[active_target][2]<3.0f) {
  151.                     int hx=hm[active_target][0]*2; int hy=hm[active_target][1]*2;
  152.                     std::cout << "hx: " << hx << " hy: " << hy << " ";
  153.                     // Horizontal
  154.                     if (hx<320-20) {
  155.                         unsigned char cbuf[17];
  156.                         cbuf[0]=0x00; cbuf[1]=0x02; cbuf[2]=0x04; for (int j=3;j<=16;j++) { cbuf[j]=0x00; }
  157.                         int retval=0;
  158.                         retval=hid_write(handle,cbuf,65);
  159.                         std::cout << timeGetTime() << ": LEFT " << retval << std::endl;
  160.                     } else if (hx>320-20) {
  161.                         unsigned char cbuf[17];
  162.                         cbuf[0]=0x00; cbuf[1]=0x02; cbuf[2]=0x08; for (int j=3;j<=16;j++) { cbuf[j]=0x00; }
  163.                         int retval=0;
  164.                         retval=hid_write(handle,cbuf,65);
  165.                         std::cout << timeGetTime() << ": RIGHT " << retval << std::endl;
  166.                     } else if (hy<240-20) { // Vertical
  167.                         unsigned char cbuf[17];
  168.                         cbuf[0]=0x00; cbuf[1]=0x02; cbuf[2]=0x02; for (int j=3;j<=16;j++) { cbuf[j]=0x00; }
  169.                         int retval=0;
  170.                         retval=hid_write(handle,cbuf,65);
  171.                         std::cout << timeGetTime() << ": UP " << retval << std::endl;
  172.                     } else if (hy>240-20) {
  173.                         unsigned char cbuf[17];
  174.                         cbuf[0]=0x00; cbuf[1]=0x02; cbuf[2]=0x01; for (int j=3;j<=16;j++) { cbuf[j]=0x00; }
  175.                         int retval=0;
  176.                         retval=hid_write(handle,cbuf,65);
  177.                         std::cout << timeGetTime() << ": DOWN " << retval << std::endl;
  178.                     }
  179.  
  180.                     // Fire
  181.                     if (timeGetTime()-last_fire_time>5000 && hx>=320-20 && hx<=320+20 && hy>=240-20 && hy<=240-20) {
  182.                         unsigned char cbuf[17];
  183.                         // Stop first!
  184.                         cbuf[0]=0x00; cbuf[1]=0x02; for (int j=2;j<=16;j++) { cbuf[j]=0x00; }
  185.                         int retval=hid_write(handle,cbuf,65);
  186.                         std::cout << timeGetTime() << ": FIRESTOP " << retval << std::endl;
  187.  
  188.                         cbuf[0]=0x00; cbuf[1]=0x02; cbuf[2]=0x10; for (int j=3;j<=16;j++) { cbuf[j]=0x00; }
  189.                         retval=hid_write(handle,cbuf,65);
  190.                         std::cout << timeGetTime() << ": FIRE " << retval << std::endl;
  191.                         last_fire_time=timeGetTime();
  192.                     }
  193.  
  194.                     // Sleep and stop, to limit movement
  195.                     if (timeGetTime()-last_fire_time>5000) {
  196.                         Sleep(20);
  197.                         unsigned char cbuf[17];
  198.                         cbuf[0]=0x00; cbuf[1]=0x02; for (int j=2;j<=16;j++) { cbuf[j]=0x00; }
  199.                         int retval=hid_write(handle,cbuf,65);
  200.                         std::cout << timeGetTime() << ": STOP " << retval << std::endl;
  201.                     }
  202.                 }
  203.                 last_cmd_time=timeGetTime();
  204.             }
  205.         }
  206.  
  207.         if (waitKey(30) == 27) {
  208.             std::cout << "Exiting..." << endl;
  209.             unsigned char cbuf[17];
  210.             cbuf[0]=0x00; cbuf[1]=0x02; for (int j=2;j<=16;j++) { cbuf[j]=0x00; }
  211.             int retval=hid_write(handle,cbuf,65);
  212.             std::cout << timeGetTime() << ": EXITSTOP " << retval << std::endl;
  213.             break;
  214.         }
  215.     }
  216.     return 0;
  217.  
  218. }
  219.  
  220.  
  221. void drawRect(cv::Mat &background, int sx, int sy, int width, int height, int col_r, int col_g, int col_b) {
  222.     Vec3b color; color[0]=col_r; color[1]=col_g; color[2]=col_b;
  223.  
  224.     for (int x=sx; x<sx+width; ++x) {
  225.         if (x>0 && x<background.cols-1) {
  226.             if (sy>0 && sy<background.rows-1)                   background.at<Vec3b>(Point(x,sy))=color;
  227.             if (sy+height-1>0 && sy+height-1<background.rows-1) background.at<Vec3b>(Point(x,sy+height-1))=color;
  228.         }
  229.     }
  230.    
  231.     for (int y=sy; y<sy+height; ++y) {
  232.         if (y>0 && y<background.rows-1) {
  233.             if (sx>0 && sx<background.cols-1)                   background.at<Vec3b>(Point(sx,y))=color;
  234.             if (sx+width-1>0 && sx+width-1<background.cols-1)   background.at<Vec3b>(Point(sx+width-1,y))=color;
  235.         }
  236.     }
  237. }
  238.  
  239. void obtainTargets(const cv::Mat &background, const std::vector<cv::Mat> &hostiles, const std::vector<cv::Mat> &hostiles_2, std::vector<std::vector<float > > &hmatch) {
  240.     //std::cout << "b.rows: " << background.rows << " b.cols: " << background.cols << std::endl;
  241.     //std::cout << "h.rows: " << hostiles[0].rows << " h.cols: " << hostiles[0].cols << std::endl;
  242.  
  243.     Mat background_2; resize(background, background_2, Size(160,120), 0, 0, INTER_CUBIC);
  244.  
  245.     std::vector<std::vector<bool > > p; p.resize(320); for (int x=0;x<320;x++) { p[x].resize(240); }
  246.     for (int x=0;x<320;x++) { for (int y=0;y<240;y++) { p[x][y]=false; }}
  247.  
  248.     // Obtain candidates by hierarchical decomposition, to avoid convolution of full tile over all pixels
  249.     // Do a coarse 7x7 on 160x120, step 2x2
  250.     for (int y=7*2; y<background_2.rows-7*2; y+=2) {
  251.         for (int x=7*2; x<background_2.cols-7*2; x+=2) {
  252.             Mat s(background_2, cv::Rect(x,y,7,7));
  253.  
  254.             float best_err=1000.0f;
  255.             for (int i=0;i<6;i++) {
  256.                 float err=(norm(s,hostiles_2[i],CV_L2))/static_cast<double>(s.rows*s.cols);
  257.                 if (err<best_err) { best_err=err; }
  258.             }
  259.  
  260.             if (best_err<30.0f) {
  261.                 for (int x2=x*2;x2<x*2+4;x2++) {
  262.                     for (int y2=y*2;y2<y*2+4;y2++) {
  263.                         p[x2][y2]=true;
  264.                     }
  265.                 }
  266.             }
  267.         }
  268.     }
  269.  
  270.     for (int y=0; y<background.rows-13; y+=2) {
  271.         for (int x=0; x<background.cols-13; x+=2) {
  272.             if (p[x][y]) {
  273.                 Mat s(background, cv::Rect(x,y,13,13));
  274.  
  275.                 float best_err=1000;
  276.                 for (int i=0;i<6;i++) {
  277.                     float err=(norm(s,hostiles[i],CV_L2))/static_cast<double>(s.rows*s.cols);
  278.  
  279.                     if (err<hmatch[i][2]) {
  280.                         hmatch[i][2]=err;
  281.                         hmatch[i][0]=x; hmatch[i][1]=y;
  282.                     }
  283.  
  284.                     if (err<best_err) { best_err=err; }
  285.                 }
  286.             }
  287.         }
  288.     }
  289. }
  290.  
  291. // From http://jepsonsblog.blogspot.in/2012/10/overlay-transparent-image-in-opencv.html
  292. void overlayImage(const cv::Mat &background, const cv::Mat &foreground, cv::Mat &output, cv::Point2i location) {
  293.     background.copyTo(output);
  294.  
  295.     int maxy=0; if (location.y>0) { maxy=location.y; }
  296.     for (int y=maxy; y<background.rows; ++y) {
  297.         int fY=y-location.y; // because of the translation
  298.         if(fY>=foreground.rows) break;
  299.  
  300.         int maxx=0; if (location.x>0) { maxy=location.x; }
  301.         for (int x=maxx; x<background.cols; ++x) {
  302.             int fX=x-location.x;
  303.             if (fX>=foreground.cols) break;
  304.  
  305.             double opacity=((double)foreground.data[fY * foreground.step + fX * foreground.channels() + 3])/255.;
  306.  
  307.             for (int c=0; opacity>0 && c<output.channels(); ++c) {
  308.                 unsigned char foregroundPx=foreground.data[fY*foreground.step+fX*foreground.channels()+c];
  309.                 unsigned char backgroundPx=background.data[y*background.step+x*background.channels()+c];
  310.                 output.data[y*output.step+output.channels()*x+c]=backgroundPx*(1.-opacity)+foregroundPx*opacity;
  311.             }
  312.         }
  313.     }
  314. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!