Remove rect joining, bounding box cleaning, control smoothing
This commit is contained in:
lhark
parent
db2c5fb3bf
commit
210b762ffe
3 changed files with 58 additions and 61 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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<cv::Mat> 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<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);
|
||||
for (const auto& rect : c_rects)
|
||||
cv::rectangle(output, rect, cv::Scalar(0, 255, 0), 2);
|
||||
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::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);
|
||||
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<cv::Rect> nc_rects, vector<cv::Rect> &c_rects) {
|
||||
int* blocs_id = new int[nc_rects.size()];
|
||||
int val;
|
||||
void cleanBBoxes(vector<cv::Rect> nc_rects, cv::Size img, vector<cv::Rect> &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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
if (center.y > img.height * 2 / 3 || center.y < img.height / 3)
|
||||
continue;
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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) {
|
||||
|
|
|
|||
Loading…
Reference in a new issue