Cal2017.cpp

/\*

Created to eliminate the plugging in and out of the usb ports

by Ivan16/17

\*/

//Include Libraries

#include <stdlib.h>

#include <iostream>

#include <vector>

#include <stdio.h>

#include <string>

#include <opencv2/imgproc/imgproc.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/opencv.hpp>

#include "ros/ros.h"

#include "std\_msgs/String.h"

#include "std\_msgs/Int16.h"

#include "geometry\_msgs/Point32.h"

#include <boost/lexical\_cast.hpp>

#include <math.h>

#include <sstream>

//Declarations

using namespace cv;

int state = 0;

int count = 0;

int hitflare = 0;

bool ifgui = true;

Mat hsvbottom;

Mat hsvfront;

int H;

int S;

int V;

int bot\_L\_r;

int bot\_L\_g;

int bot\_L\_b;

int bot\_U\_r;

int bot\_U\_g;

int bot\_U\_b;

int number = 0;

int posX = 0;

int posY = 1;

Moments oMoments;

double dArea;

double dM01;

double dM10;

int countF;

int countB;

int main(int argc, char \*\*argv)

{

//SETUP

ros::init(argc, argv, "auv\_camera");

ros::NodeHandle n;

//ros::Publisher camera\_pub\_yl = n.advertise<geometry\_msgs::Point32>("cam\_yellow", 1000);

//ros::Publisher camera\_pub\_state = n.advertise<std\_msgs::String>("cam\_state", 1000);

ros::Publisher camera\_pub\_angle = n.advertise<std\_msgs::String>("cam\_angle", 1000);

ros::Rate loop\_rate(10);

std\_msgs::String cam\_angle;

//geometry\_msgs::Point32 cam\_black;

//geometry\_msgs::Point32 cam\_red;

//geometry\_msgs::Point32 cam\_blue;

//Initialise Bottom Camera

FILE \* pfile;

pfile = fopen("ivan.txt","w");

VideoCapture cambottom(0);

if(!cambottom.isOpened()) //Checking sequence

{

return -1;

}

cambottom.set(CV\_CAP\_PROP\_FRAME\_WIDTH, 320);

cambottom.set(CV\_CAP\_PROP\_FRAME\_HEIGHT, 240);

//Initialise Front Camera

VideoCapture camfront(1);

if(!camfront.isOpened()) //Checking sequence

{

return -1;

}

camfront.set(CV\_CAP\_PROP\_FRAME\_WIDTH, 320);

camfront.set(CV\_CAP\_PROP\_FRAME\_HEIGHT, 240);

char color = 'Y';

int iLowH = 29;

int iHighH = 88;

int iLowS = 51;

int iHighS = 187;

int iLowV = 113;

int iHighV = 188;

// if(ifgui) namedWindow("Original Frame (Bottom)",1);

//if(ifgui) namedWindow("Thresholded Frame (Bottom)",1);

//cam\_state.data="black";

//camera\_pub\_state.publish(cam\_state);

while(1)

{

count += 1;

//pfile = fopen("results.txt","w");

Mat framebottom;

cambottom >> framebottom; // Get a new frame from camera

GaussianBlur(framebottom, framebottom, Size(7,7), 1.5, 1.5);

cvtColor(framebottom, hsvbottom, CV\_BGR2HSV);

Mat imgThresholdedB;

inRange(framebottom, Scalar(iLowH,iLowS,iLowV), Scalar(iHighH,iHighS,iHighV), imgThresholdedB);

erode(imgThresholdedB, imgThresholdedB,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));                //Eliminate the small particles

dilate(imgThresholdedB, imgThresholdedB,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

erode(imgThresholdedB, imgThresholdedB,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

dilate(imgThresholdedB, imgThresholdedB,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

oMoments = moments(imgThresholdedB);

dM01 = oMoments.m01;

dM10 = oMoments.m10;

dArea = oMoments.m00;

if(dArea >20000)

{

printf("Bottom camera deteced \n");

countB +=1;

}else{

printf("Bottom not detected \n");

fprintf(pfile,"Bottom not detected \n");

}

unsigned char \*input = (unsigned char\*)(imgThresholdedB.data);

if(ifgui) imshow("Original Frame (Bottom)", framebottom);

if(ifgui) imshow("[Black] Thresholded Frame (Bottom)", imgThresholdedB);

if(waitKey(14) >= 0) break;

Mat framefront;

camfront >> framefront; // Get a new frame from camera

GaussianBlur(framefront, framefront, Size(7,7), 1.5, 1.5);

cvtColor(framefront, hsvbottom, CV\_BGR2HSV);

Mat imgThresholdedF;

        inRange(framefront, Scalar(iLowH,iLowS,iLowV), Scalar(iHighH,iHighS,iHighV), imgThresholdedF);

erode(imgThresholdedF, imgThresholdedF,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));                //Eliminate the small particles

dilate(imgThresholdedF, imgThresholdedF,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

erode(imgThresholdedF, imgThresholdedF,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

dilate(imgThresholdedF, imgThresholdedF,getStructuringElement(MORPH\_ELLIPSE, Size(5,5)));

oMoments = moments(imgThresholdedF);

dM01 = oMoments.m01;

dM10 = oMoments.m10;

dArea = oMoments.m00;

if(dArea >20000)

{

printf("Front camera detected \n");

countF +=1;

}else{

printf("Front not detected \n");

}

if(ifgui) imshow("Original Frame (Front)", framefront);

if(ifgui) imshow("[Black] Thresholded Frame (Front)", imgThresholdedF);

if(countB >30)

{

cam\_angle.data="bottom";

camera\_pub\_angle.publish(cam\_angle);

}

if(countF >30)

{

cam\_angle.data="front";

camera\_pub\_angle.publish(cam\_angle);

}

if(waitKey(15) >= 0) {

fclose(pfile); break;}

}

// the camera will be deinitialized automatically in VideoCapture destructor

return 0;

}