diff --git a/commandes b/commandes index f6338f2..5ddc3ab 100644 --- a/commandes +++ b/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 diff --git a/src/control.cpp b/src/control.cpp index d4563e2..7ce4e37 100644 --- a/src/control.cpp +++ b/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 decision_acc; + const static int NB_ACC = 5; + Drone_control() : n("~") { pub_cmd = n.advertise("/bebop/cmd_vel", 1); pub_takeoff = n.advertise("/bebop/takeoff", 1); pub_land = n.advertise("/bebop/land", 1); sub_box = n.subscribe("/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"); } }; diff --git a/src/papillon.cpp b/src/papillon.cpp index 068a250..18d0838 100644 --- a/src/papillon.cpp +++ b/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 ERODE_SIZE = 5; - const static int NB_FRAME_DROP = 3; - constexpr static double CONNECT_MAX_DIST = 40.0; + const static int CLOSE_SIZE = 30; + const static int ERODE_SIZE = 2; + const static int NB_FRAME_DROP = 1; vector prevs; @@ -200,25 +199,27 @@ 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 nc_rects; // Non connected rectangles - for(int i=0; i c_rects; // Connected rectangles - connectBBox(nc_rects, c_rects); - 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::rectangle(output, objBRect, cv::Scalar(0, 255, 0), 2); - papillon::BoundingBox bbox = papillon::BoundingBox(); - bbox.x = objBRect.x / (float)cur.size().width; - bbox.y = objBRect.y / (float)cur.size().height; - bbox.width = objBRect.width / (float)cur.size().width; - bbox.height = objBRect.height / (float)cur.size().height; - pub_cmd.publish(bbox); + 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 = 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; + bbox.y = objBRect.y / (float)cur.size().height; + bbox.width = objBRect.width / (float)cur.size().width; + 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; @@ -234,47 +235,24 @@ class Traite_image { } - void connectBBox(vector nc_rects, vector &c_rects) { - int* blocs_id = new int[nc_rects.size()]; - int val; - - // We find all the bounding boxes that are close enough to each other - // And we sort them into teams (blocs_id) - 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; - vector* sorted_rects = new vector[maximum]; - for (int i = 0; i < maximum; ++i) { - sorted_rects[i].push_back(nc_rects[i]); - } - for (int i = 0; i < maximum; ++i) { - c_rects.push_back(sorted_rects[i].at(0)); - for (const auto& rect : sorted_rects[i]) { - c_rects.back() |= rect; - } - } + void cleanBBoxes(vector nc_rects, cv::Size img, vector &c_rects) { + int max = 0; + for (auto r : nc_rects) { + cv::Point center = rectCenter(r); + if (r.width > img.width / 2 || r.height > img.height / 8) + continue; - delete [] sorted_rects; - } + if (center.y > img.height * 2 / 3 || center.y < img.height / 3) + continue; - 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]; + int r_area = r.area(); + if (max < r_area) { + max = r_area; + c_rects.insert(c_rects.begin(), r); + } else { + c_rects.push_back(r); } - return max; + } } cv::Point rectCenter(cv::Rect r) {