56 lines
1.2 KiB
C++
56 lines
1.2 KiB
C++
#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;
|
|
}
|
|
|