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;
}