Add drone control function

src/control.cpp

@@ -10,13 +10,13 @@ using namespace std;
 
 class Drone_control {
 	public:
-		cv::Rect objectBoundingRectangle = cv::Rect(0,0,0,0);
-
 		ros::NodeHandle n;
-
 		ros::Publisher pub_cmd;
 		ros::Subscriber sub_box;
 
+		float THRES_TURN = 0.1;
+		bool emergency = false;
+
 
 		Drone_control() : n("~") {
 			pub_cmd = n.advertise<geometry_msgs::Twist>("/bebop/cmd_vel", 1);
@@ -27,7 +27,19 @@ class Drone_control {
 
 		// This processes an image and publishes the result.
 		void on_msg(const papillon::BoundingBox::ConstPtr& bbox) {
-			ROS_INFO("plop");
+			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) {
+				// Image is 1.0 in width and height
+				if (r_center.x < 0.5 - THRES_TURN)
+					twist.angular.z = 0.2;
+				else if (r_center.x > 0.5 + THRES_TURN)
+					twist.angular.z = -0.2;
+			}
+
+			pub_cmd.publish(twist);
 		}
 };