Remove rect joining, bounding box cleaning, control smoothing

commandes

@@ -2,4 +2,5 @@ rosrun demo_teleop key_teleop.py --persist /cmd_vel:=/bebop/cmd_vel
 rostopic pub --once /bebop/land std_msgs/Empty
 rostopic pub --once /bebop/takeoff std_msgs/Empty
 catkin_make --pkg=papillon
+catkin_make -DCMAKE_BUILD_TYPE=Debug
 roslaunch bebop_driver bebop_node.launch

src/control.cpp

@@ -22,12 +22,21 @@ class Drone_control {
 		float THRES_TURN = 0.1;
 		bool emergency = false;
 
+		// 1 left | 0 center | -1 right
+		vector<int> decision_acc;
+		const static int NB_ACC = 5;
+
 
 		Drone_control() : n("~") {
 			pub_cmd = n.advertise<geometry_msgs::Twist>("/bebop/cmd_vel", 1);
 			pub_takeoff = n.advertise<std_msgs::Empty>("/bebop/takeoff", 1);
 			pub_land = n.advertise<std_msgs::Empty>("/bebop/land", 1);
 			sub_box = n.subscribe<papillon::BoundingBox>("/papillon/bbox", 1, &Drone_control::on_msg, this);
+
+			// Initialise decision accumulator
+			for (size_t i = 0; i < NB_ACC; ++i)
+				decision_acc.push_back(0);
+
 			ROS_INFO("Built !");
 		}
 
@@ -36,14 +45,20 @@ class Drone_control {
 		void on_msg(const papillon::BoundingBox::ConstPtr& bbox) {
 			geometry_msgs::Twist twist = geometry_msgs::Twist();
 			cv::Point2f r_center = cv::Point2f(bbox->x + bbox->width/2, bbox->y + bbox->height/2);
-			ROS_INFO("r_center.x = %f", r_center.x);
-
 			if (!emergency) {
+				decision_acc.pop_back();
 				// Image is 1.0 in width and height
 				if (r_center.x < 0.5 - THRES_TURN)
-					twist.angular.z = 0.2;
+					decision_acc.insert(decision_acc.begin(), 1);
 				else if (r_center.x > 0.5 + THRES_TURN)
-					twist.angular.z = -0.2;
+					decision_acc.insert(decision_acc.begin(), -1);
+				else
+					decision_acc.insert(decision_acc.begin(), 0);
+
+				int moy = 0;
+				for (const auto& d : decision_acc)
+					moy += d;
+				twist.angular.z = moy / (float)NB_ACC * 0.3;
 			}
 
 			pub_cmd.publish(twist);
@@ -55,7 +70,7 @@ class Drone_control {
 			keypad(stdscr, TRUE);
 			cbreak();
 			noecho();
-			//timeout(mKeyTimeout);
+			timeout(0); // Non blocking getch()
 			// <<<<< nCurses initization
 
 			int c;
@@ -82,6 +97,7 @@ class Drone_control {
 					// Takeoff
 					ROS_DEBUG_STREAM("Taking off...\r");
 					pub_takeoff.publish(nope);
+					emergency = false;
 					printw("Taking off...\r");
 					break;
 				}
@@ -89,8 +105,9 @@ class Drone_control {
 				{
 					// Takeoff
 					ROS_DEBUG_STREAM("Landing...\r");
-					pub_takeoff.publish(nope);
+					pub_land.publish(nope);
 					printw("Landing...\r");
+					emergency = true;
 					break;
 				}
 				case 'q':
@@ -102,6 +119,7 @@ class Drone_control {
 				}
 				ros::spinOnce();
 			}
+			printw("\nExiting\n");
 		}
 };
 

src/papillon.cpp

@@ -12,12 +12,11 @@ using namespace std;
 
 class Traite_image {
 	public:
-		const static int SENSITIVITY_VALUE = 35;
+		const static int SENSITIVITY_VALUE = 20;
 		const static int BLUR_Size = 9;
-		const static int CLOSE_SIZE = 20;
+		const static int CLOSE_SIZE = 30;
-		const static int ERODE_SIZE = 5;
+		const static int ERODE_SIZE = 2;
-		const static int NB_FRAME_DROP = 3;
+		const static int NB_FRAME_DROP = 1;
-		constexpr static double CONNECT_MAX_DIST = 40.0;
 
 
 		vector<cv::Mat> prevs;
@@ -200,18 +199,19 @@ class Traite_image {
 				//make a bounding rectangle around the largest contour then find its centroid
 				//this will be the object's final esticv::Mated position.
 				vector<cv::Rect> nc_rects; // Non connected rectangles
-				for(int i=0; i<contours.size();i++)
+				for(size_t i=0; i<contours.size();i++)
 				{
 					nc_rects.push_back(cv::boundingRect(contours[i]));
 					//cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);
 				}
 
 				vector<cv::Rect> c_rects; // Connected rectangles
-				connectBBox(nc_rects, c_rects);
+				cleanBBoxes(nc_rects, cur.size(), c_rects);
+				if (c_rects.size() > 0) {
 					for (const auto& rect : c_rects)
 						cv::rectangle(output, rect, cv::Scalar(0, 255, 0), 2);
 
-				cv::Rect objBRect = cv::boundingRect(contours.at(contours.size()-1));
+					cv::Rect objBRect = c_rects.front();
 					//cv::rectangle(output, objBRect, cv::Scalar(0, 255, 0), 2);
 					papillon::BoundingBox bbox = papillon::BoundingBox();
 					bbox.x = objBRect.x / (float)cur.size().width;
@@ -220,6 +220,7 @@ class Traite_image {
 					bbox.height = objBRect.height / (float)cur.size().height;
 					pub_cmd.publish(bbox);
 				}
+			}
 			//make some temp x and y variables so we dont have to type out so much
 			//~ int x = objectBoundingRectangle.x;
 			//~ int y = objectBoundingRectangle.y;
@@ -234,47 +235,24 @@ class Traite_image {
 		}
 
 
-		void connectBBox(vector<cv::Rect> nc_rects, vector<cv::Rect> &c_rects) {
+		void cleanBBoxes(vector<cv::Rect> nc_rects, cv::Size img, vector<cv::Rect> &c_rects) {
-			int* blocs_id = new int[nc_rects.size()];
+			int max = 0;
-			int val;
+			for (auto r : nc_rects) {
+				cv::Point center = rectCenter(r);
+				if (r.width > img.width / 2 || r.height > img.height / 8)
+					continue;
 
-			// We find all the bounding boxes that are close enough to each other
+				if (center.y > img.height * 2 / 3 || center.y < img.height / 3)
-			// And we sort them into teams (blocs_id)
+					continue;
-			for (int i = 0; i < nc_rects.size() - 2; ++i) {
-				for (int j = i + 1; j < nc_rects.size() - 1; ++j) {
-					if (cv::norm(rectCenter(nc_rects.at(i)) - rectCenter(nc_rects.at(j))) < CONNECT_MAX_DIST) {
-						val = min(blocs_id[i], blocs_id[j]);
-						blocs_id[j] = blocs_id[i] = val;
-					} else if (blocs_id[i] == blocs_id[j]) {
-						blocs_id[j] = i + 1;
-					}
-				}
-			}
 
-			int maximum = maxElement(blocs_id,nc_rects.size()) + 1;
+				int r_area = r.area();
-			vector<cv::Rect>* sorted_rects = new vector<cv::Rect>[maximum];
+				if (max < r_area) {
-			for (int i = 0; i < maximum; ++i) {
+					max = r_area;
-				sorted_rects[i].push_back(nc_rects[i]);
+					c_rects.insert(c_rects.begin(), r);
-			}
+				} else {
-			for (int i = 0; i < maximum; ++i) {
+					c_rects.push_back(r);
-				c_rects.push_back(sorted_rects[i].at(0));
-				for (const auto& rect : sorted_rects[i]) {
-					c_rects.back() |= rect;
 				}
 			}
-
-			delete [] sorted_rects;
-		}
-
-		int maxElement(int Array[], int array_size)
-		{
-			int max = Array[0];
-			for (int i = 1; i < array_size; i++)
-			{
-					if (Array[i] > max)
-						max = Array[i];
-				}
-			return max;
 		}
 
 		cv::Point rectCenter(cv::Rect r) {