lhark
/
papillon
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

Remove rect joining, bounding box cleaning, control smoothing

master
lhark 8 years ago
parent
db2c5fb3bf
commit
210b762ffe
3 changed files with 59 additions and 62 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      commandes
  2. 30
      src/control.cpp
  3. 90
      src/papillon.cpp

1
commandes
View File

@ -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/land std_msgs/Empty
rostopic pub --once /bebop/takeoff std_msgs/Empty rostopic pub --once /bebop/takeoff std_msgs/Empty
catkin_make --pkg=papillon catkin_make --pkg=papillon
catkin_make -DCMAKE_BUILD_TYPE=Debug
roslaunch bebop_driver bebop_node.launch roslaunch bebop_driver bebop_node.launch

30
src/control.cpp
View File

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

90
src/papillon.cpp
View File

@ -12,12 +12,11 @@ using namespace std;
class Traite_image { class Traite_image {
public: public:
const static int SENSITIVITY_VALUE = 35; const static int SENSITIVITY_VALUE = 20;
const static int BLUR_Size = 9; 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; vector<cv::Mat> prevs;
@ -200,25 +199,27 @@ class Traite_image {
//make a bounding rectangle around the largest contour then find its centroid //make a bounding rectangle around the largest contour then find its centroid
//this will be the object's final esticv::Mated position. //this will be the object's final esticv::Mated position.
vector<cv::Rect> nc_rects; // Non connected rectangles 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])); nc_rects.push_back(cv::boundingRect(contours[i]));
//cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2); //cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);
} }
vector<cv::Rect> c_rects; // Connected rectangles vector<cv::Rect> c_rects; // Connected rectangles
connectBBox(nc_rects, c_rects); cleanBBoxes(nc_rects, cur.size(), c_rects);
for (const auto& rect : c_rects) if (c_rects.size() > 0) {
cv::rectangle(output, rect, cv::Scalar(0, 255, 0), 2); 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); cv::Rect objBRect = c_rects.front();
papillon::BoundingBox bbox = papillon::BoundingBox(); //cv::rectangle(output, objBRect, cv::Scalar(0, 255, 0), 2);
bbox.x = objBRect.x / (float)cur.size().width; papillon::BoundingBox bbox = papillon::BoundingBox();
bbox.y = objBRect.y / (float)cur.size().height; bbox.x = objBRect.x / (float)cur.size().width;
bbox.width = objBRect.width / (float)cur.size().width; bbox.y = objBRect.y / (float)cur.size().height;
bbox.height = objBRect.height / (float)cur.size().height; bbox.width = objBRect.width / (float)cur.size().width;
pub_cmd.publish(bbox); 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 //make some temp x and y variables so we dont have to type out so much
//~ int x = objectBoundingRectangle.x; //~ int x = objectBoundingRectangle.x;
@ -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);
// We find all the bounding boxes that are close enough to each other if (r.width > img.width / 2 || r.height > img.height / 8)
// 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;
vector<cv::Rect>* sorted_rects = new vector<cv::Rect>[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;
}
}
delete [] sorted_rects; if (center.y > img.height * 2 / 3 || center.y < img.height / 3)
} continue;
int maxElement(int Array[], int array_size) int r_area = r.area();
{ if (max < r_area) {
int max = Array[0]; max = r_area;
for (int i = 1; i < array_size; i++) c_rects.insert(c_rects.begin(), r);
{ } else {
if (Array[i] > max) c_rects.push_back(r);
max = Array[i];
} }
return max; }
} }
cv::Point rectCenter(cv::Rect r) { cv::Point rectCenter(cv::Rect r) {

Write Preview
Loading…
Cancel
Save