From c60831353054dab0be296e7ad92ebe62443d1a04 Mon Sep 17 00:00:00 2001
From: lhark <lhark@hotmail.fr>
Date: Thu, 2 Jun 2016 18:06:24 +0200
Subject: [PATCH] A bit of cleaning, simul arg

---
 src/papillon.cpp | 38 +++++++++++++++++++++++---------------
 1 file changed, 23 insertions(+), 15 deletions(-)

diff --git a/src/papillon.cpp b/src/papillon.cpp
index 1e2d758..8d7dac7 100644
--- a/src/papillon.cpp
+++ b/src/papillon.cpp
@@ -17,12 +17,13 @@ class Traite_image {
 		const static int THRESHOLD_DETECT_SENSITIVITY = 10;
 		const static int BLUR_SIZE = 5;
         const static int THRESHOLD_MOV = 5;
+		const static int crop_ratio = 8;
 
 
 		Mat prev;
 		Mat last_T;
 		bool first = true;
-		int resize_f = 2;
+		int resize_f = 1;
 
 		int theObject[2] = {0,0};
 		Rect objectBoundingRectangle = Rect(0,0,0,0);
@@ -35,10 +36,22 @@ class Traite_image {
 		image_transport::Subscriber     sub;
 
 
-		Traite_image() : n("~"),it(n) {
-			pub_img = it.advertise("/image_out", 1);
-			pub_cmd = n.advertise<geometry_msgs::Twist>("/vrep/drone/cmd_vel", 1);
-			sub = it.subscribe("/usb_cam/image_raw", 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
+		Traite_image(bool sim) : n("~"),it(n) {
+			String img_out, cmd_out, img_in;
+			if (!sim) {
+				img_out = "/image_out";
+				cmd_out = "/bebop/cmd_vel";
+				img_in = "/bebop/image_raw";
+			}
+			else
+			{
+				img_out = "/image_out";
+				cmd_out = "/vrep/drone/cmd_vel";
+				img_in = "/usb_cam/image_raw";
+			}
+			pub_img = it.advertise(img_out, 1);
+			pub_cmd = n.advertise<geometry_msgs::Twist>(cmd_out, 1);
+			sub = it.subscribe(img_in, 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
 		}
 
 
@@ -69,7 +82,6 @@ class Traite_image {
 
 			Mat next_stab;
 			stabiliseImg(prev, next, next_stab);
-			int crop_ratio = 6;
 			float crop_x = next_stab.size().width/crop_ratio;
 			float crop_y = next_stab.size().height/crop_ratio;
 			float crop_w = next_stab.size().width*(1-2.0/crop_ratio);
@@ -77,7 +89,7 @@ class Traite_image {
 			Rect myROI(crop_x, crop_y, crop_w, crop_h);
 			Mat next_stab_cropped = next_stab(myROI);
 			Mat prev_cropped = prev(myROI);
-			searchForMovement(prev_cropped, next_stab_cropped, output);
+			searchForMovementOptFlow(prev_cropped, next_stab_cropped, output);
 
 
 			pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
@@ -207,7 +219,6 @@ class Traite_image {
 			threshold(flow_norm, flow_norm, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
             flow_norm.convertTo(flow_norm, CV_8U);
 
-            bool objectDetected = false;
 			Mat temp;
 			flow_norm.copyTo(temp);
 			//these two vectors needed for output of findContours
@@ -217,11 +228,7 @@ class Traite_image {
 			//findContours(temp,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );// retrieves all contours
 			findContours(temp,contours,hierarchy,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE );// retrieves external contours
 
-			//if contours vector is not empty, we have found some objects
-			if(contours.size()>0)objectDetected=true;
-			else objectDetected = false;
-
-			if(objectDetected){
+			if(contours.size()>0){ //if contours vector is not empty, we have found some objects
 				//the largest contour is found at the end of the contours vector
 				//we will simply assume that the biggest contour is the object we are looking for.
 				vector< vector<Point> > largestContourVec;
@@ -273,8 +280,9 @@ class Traite_image {
 
 int main(int argc, char **argv)
 {
-	ros::init(argc, argv, "test_opencv");
-	Traite_image dataset=Traite_image();
+	ros::init(argc, argv, "Papillon");
+	bool sim = false;
+	Traite_image dataset=Traite_image(sim);
 	ros::spin();
 
 	return 0;