diff --git a/CMakeLists.txt b/CMakeLists.txt index 95c4c1c..bcb7ce8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,8 @@ find_package(catkin REQUIRED COMPONENTS message_generation ) find_package(OpenCV) +find_package( PkgConfig REQUIRED ) +pkg_check_modules ( ncurses++ REQUIRED ncurses++ ) add_message_files( FILES @@ -34,7 +36,7 @@ add_executable (papillon src/papillon.cpp) add_executable (control src/control.cpp) add_dependencies( papillon ${PROJECT_NAME}_generate_messages_cpp ) target_link_libraries(papillon ${catkin_LIBRARIES}) -target_link_libraries(control ${catkin_LIBRARIES}) +target_link_libraries(control ${catkin_LIBRARIES} ncurses) set_property (TARGET papillon APPEND PROPERTY INCLUDE_DIRECTORIES ${OpenCV_INCLUDE_DIRS}) set_property (TARGET papillon APPEND PROPERTY INCLUDE_DIRECTORIES ${catkin_INCLUDE_DIRS}) set_property (TARGET papillon APPEND PROPERTY LINK_LIBRARIES ${OpenCV_LIBRARIES}) diff --git a/package.xml b/package.xml index 7e113fa..0e9f93e 100644 --- a/package.xml +++ b/package.xml @@ -46,6 +46,7 @@ image_transport cv_bridge message_generation + ncurses++ roscpp std_msgs image_transport diff --git a/src/control.cpp b/src/control.cpp index 4fa5e33..d4563e2 100644 --- a/src/control.cpp +++ b/src/control.cpp @@ -1,6 +1,8 @@ #include "ros/ros.h" #include #include +#include +#include #include @@ -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("/bebop/cmd_vel", 1); + pub_takeoff = n.advertise("/bebop/takeoff", 1); + pub_land = n.advertise("/bebop/land", 1); sub_box = n.subscribe("/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; }