Cal2017.cpp

/*

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

by Ivan16/17

* /

//Include Libraries


 * 1) include 


 * 1) include


 * 1) include


 * 1) include 


 * 1) include


 * 1) include 


 * 1) include 


 * 1) include 


 * 1) include "ros/ros.h"


 * 1) include "std_msgs/String.h"


 * 1) include "std_msgs/Int16.h"


 * 1) include "geometry_msgs/Point32.h"


 * 1) include 


 * 1) include 


 * 1) include

//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("cam_yellow", 1000);

//ros::Publisher camera_pub_state = n.advertise("cam_state", 1000);

ros::Publisher camera_pub_angle = n.advertise("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;

}