lhark/papillon
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Add drone control function

Browse source
This commit is contained in:
lhark 2016-06-15 17:29:32 +02:00
parent 69b74cfe60
commit de402960c6
1 changed files with 16 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
src/control.cpp
View file

@ -10,13 +10,13 @@ using namespace std;
class Drone_control { class Drone_control {
public: public:
cv::Rect objectBoundingRectangle = cv::Rect(0,0,0,0);
ros::NodeHandle n; ros::NodeHandle n;
ros::Publisher pub_cmd; ros::Publisher pub_cmd;
ros::Subscriber sub_box; ros::Subscriber sub_box;
float THRES_TURN = 0.1;
bool emergency = false;
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);
@ -27,7 +27,19 @@ class Drone_control {
// This processes an image and publishes the result. // This processes an image and publishes the result.
void on_msg(const papillon::BoundingBox::ConstPtr& bbox) { 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);
} }
}; };