|
|
@ -1,6 +1,8 @@ |
|
|
|
#include "ros/ros.h" |
|
|
|
#include <papillon/BoundingBox.h> |
|
|
|
#include <geometry_msgs/Twist.h> |
|
|
|
#include <std_msgs/Empty.h> |
|
|
|
#include <ncurses.h> |
|
|
|
|
|
|
|
#include <opencv/cv.h> |
|
|
|
|
|
|
@ -12,7 +14,10 @@ class Drone_control { |
|
|
|
public: |
|
|
|
ros::NodeHandle n; |
|
|
|
ros::Publisher pub_cmd; |
|
|
|
ros::Publisher pub_takeoff; |
|
|
|
ros::Publisher pub_land; |
|
|
|
ros::Subscriber sub_box; |
|
|
|
std_msgs::Empty nope; |
|
|
|
|
|
|
|
float THRES_TURN = 0.1; |
|
|
|
bool emergency = false; |
|
|
@ -20,6 +25,8 @@ class Drone_control { |
|
|
|
|
|
|
|
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); |
|
|
|
ROS_INFO("Built !"); |
|
|
|
} |
|
|
@ -41,6 +48,61 @@ class Drone_control { |
|
|
|
|
|
|
|
pub_cmd.publish(twist); |
|
|
|
} |
|
|
|
|
|
|
|
void key_spin() { |
|
|
|
// >>>>> nCurses initization
|
|
|
|
initscr(); |
|
|
|
keypad(stdscr, TRUE); |
|
|
|
cbreak(); |
|
|
|
noecho(); |
|
|
|
//timeout(mKeyTimeout);
|
|
|
|
// <<<<< nCurses initization
|
|
|
|
|
|
|
|
int c; |
|
|
|
|
|
|
|
printw(" ------------------------\n"); |
|
|
|
printw("| Papillon HQ |\n"); |
|
|
|
printw(" ------------------------ \n"); |
|
|
|
printw("| Press F to take off. |\n"); |
|
|
|
printw("| Press SPACEBAR to land.|\n"); |
|
|
|
printw("| Press Q to exit. |\n"); |
|
|
|
printw(" ------------------------\n"); |
|
|
|
|
|
|
|
bool stop = false; |
|
|
|
|
|
|
|
ros::Rate rate(1.0/30.0); // 30 Hz
|
|
|
|
while(!stop) |
|
|
|
{ |
|
|
|
c = getch(); |
|
|
|
|
|
|
|
switch(c) |
|
|
|
{ |
|
|
|
case 'f': |
|
|
|
{ |
|
|
|
// Takeoff
|
|
|
|
ROS_DEBUG_STREAM("Taking off...\r"); |
|
|
|
pub_takeoff.publish(nope); |
|
|
|
printw("Taking off...\r"); |
|
|
|
break; |
|
|
|
} |
|
|
|
case ' ': |
|
|
|
{ |
|
|
|
// Takeoff
|
|
|
|
ROS_DEBUG_STREAM("Landing...\r"); |
|
|
|
pub_takeoff.publish(nope); |
|
|
|
printw("Landing...\r"); |
|
|
|
break; |
|
|
|
} |
|
|
|
case 'q': |
|
|
|
case 'Q': |
|
|
|
{ |
|
|
|
ROS_DEBUG_STREAM("EXIT\r"); |
|
|
|
stop = true; |
|
|
|
} |
|
|
|
} |
|
|
|
ros::spinOnce(); |
|
|
|
} |
|
|
|
} |
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
@ -48,7 +110,7 @@ int main(int argc, char **argv) |
|
|
|
{ |
|
|
|
ros::init(argc, argv, "control"); |
|
|
|
Drone_control con=Drone_control(); |
|
|
|
ros::spin(); |
|
|
|
con.key_spin(); |
|
|
|
|
|
|
|
return 0; |
|
|
|
} |
|
|
|