papillon/src/control.cpp

#include "ros/ros.h"
#include <papillon/BoundingBox.h>
#include <geometry_msgs/Twist.h>

#include <opencv/cv.h>

#include <sstream>

using namespace std;

class Drone_control {
	public:
		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);
			sub_box = n.subscribe<papillon::BoundingBox>("/papillon/bbox", 1, &Drone_control::on_msg, this);
			ROS_INFO("Built !");
		}


		// This processes an image and publishes the result.
		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) {
				// 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);
		}
};


int main(int argc, char **argv)
{
	ros::init(argc, argv, "control");
	Drone_control con=Drone_control();
	ros::spin();

	return 0;
}