Add ncurses interfaces to control.cpp

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

package.xml

@@ -46,6 +46,7 @@
   <build_depend>image_transport</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>ncurses++</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>image_transport</run_depend>

src/control.cpp

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