diff --git a/src/display.h b/src/display.h index 341c318..5e6edd7 100644 --- a/src/display.h +++ b/src/display.h @@ -1,5 +1,5 @@ -#ifndef CURSES_TEST -#define CURSES_TEST +#ifndef CURSES_DISPLAY +#define CURSES_DISPLAY #include @@ -9,17 +9,25 @@ class Curses static const int kbd_lines; static const int kbd_columns; WINDOW* kbd; - static const char** kbd_descr; static const int speed_lines; static const int speed_columns; WINDOW* speed; void print_kbd(); WINDOW* get; + + // TODO (avec scroll) + WINDOW* log_sent_w; + WINDOW* nav_data; public: Curses(); ~Curses(); char getchar(); + + // TODO + void update_speed(const char&, const float&); + // void update_navdata(... + void log_sent(char*); }; #endif diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index 31f118e..a523d33 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -1,8 +1,3 @@ -#include -#include -#include -#include -#include #include #include #include @@ -27,7 +22,6 @@ class Run { pub_reset.publish(empty); }; float x_speed, y_speed, z_speed, turn; boost::shared_ptr term; - void print_speed() { ; }; public: Run(boost::shared_ptr terminal) : @@ -47,6 +41,8 @@ class Run { while (ros::ok()) { + ros::spinOnce(); + geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist()); msg->linear.x = msg->linear.y = msg->linear.z = msg->angular.x = msg->angular.y = msg->angular.z = 0.; @@ -58,42 +54,49 @@ class Run case 'k' : {// hover cmd.publish(msg); + term->log_sent("hover !"); break; } case 'i' : {// forward msg->linear.x = x_speed; cmd.publish(msg); + term->log_sent("forward !"); break; } case ';' : {// backward msg->linear.x = -x_speed; cmd.publish(msg); + term->log_sent("backward !"); break; } case 'h' : {//translate left msg->linear.y = -y_speed; cmd.publish(msg); + term->log_sent("translate left !"); break; } case 'm' : {//translate right msg->linear.y = y_speed; cmd.publish(msg); + term->log_sent("translate right !"); break; } case 'j' : {//rotate left msg->angular.z = turn; cmd.publish(msg); + term->log_sent("rotate left !"); break; } case 'l' : {//rotate right msg->angular.z = -turn; cmd.publish(msg); + term->log_sent("rotate right !"); break; } case 'u' : @@ -101,6 +104,7 @@ class Run msg->angular.z = turn; msg->linear.x = x_speed; cmd.publish(msg); + term->log_sent("forward left !"); break; } case 'o' : @@ -108,6 +112,7 @@ class Run msg->angular.z = -turn; msg->linear.x = x_speed; cmd.publish(msg); + term->log_sent("forward right !"); break; } case ',' : @@ -115,6 +120,7 @@ class Run msg->angular.z = turn; msg->linear.x = -x_speed; cmd.publish(msg); + term->log_sent("backward left !"); break; } case ':' : @@ -122,100 +128,112 @@ class Run msg->angular.z = -turn; msg->linear.x = -x_speed; cmd.publish(msg); + term->log_sent("backward right !"); break; } case 'y' : {//up msg->linear.z = z_speed; cmd.publish(msg); + term->log_sent("up !"); break; } case 'n' : {//down msg->linear.z = -z_speed; cmd.publish(msg); + term->log_sent("down !"); break; } case 't' : {//takeoff takeoff(); + term->log_sent("takeoff !"); break; } case 'b' : {//land land(); + term->log_sent("land !"); break; } case 'g' : {//reset reset(); + term->log_sent("reset !"); break; } case 'a' : {// + x_speed x_speed *= 1.1; - print_speed(); + term->update_speed('x', x_speed); break; } case 'w' : {// - x_speed x_speed *= 0.9; - print_speed(); + term->update_speed('x', x_speed); break; } case 'z' : {// + y_speed y_speed *= 1.1; - print_speed(); + term->update_speed('y', y_speed); break; } case 'x' : {// - y_speed y_speed *= 0.9; - print_speed(); + term->update_speed('y', y_speed); break; } case 'e' : {// + z_speed z_speed *= 1.1; - print_speed(); + term->update_speed('z', z_speed); break; } case 'c' : {// - z_speed z_speed *= 0.9; - print_speed(); + term->update_speed('z', z_speed); break; } case 'r' : {// + turn speed turn *= 1.1; - print_speed(); + term->update_speed('t', turn); break; } case 'v' : {// - turn speed turn *= 0.9; - print_speed(); + term->update_speed('t', turn); break; } default : break; - } // switch - ros::spinOnce(); + } // switch(c) loop_rate.sleep(); } // while - } // run + } //void run() -}; // class +}; // class Run + +class Callback +{ + //TODO + // receives nav_data and display them in the term +}; // class Callback int main(int argc, char** argv) { setlocale(LC_ALL, ""); ros::init(argc, argv, "keyboard_cmd"); + //ros::Subscriber ... (navdata) boost::shared_ptr term(new Curses()); Run fun(term); fun(); return 0; -} +} // main