From 286dbb36bf1c673c92ac945c60c176954e45e908 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Fri, 1 May 2015 21:23:00 +0200 Subject: [PATCH 01/17] essai ncurses --- CMakeLists.txt | 11 +++++++++++ src/curses.cpp | 37 +++++++++++++++++++++++++++++++++++++ src/curses.h | 23 +++++++++++++++++++++++ src/keyboard_cmd.cpp | 23 +++++------------------ src/test.cpp | 8 ++++++++ 5 files changed, 84 insertions(+), 18 deletions(-) create mode 100644 src/curses.cpp create mode 100644 src/curses.h create mode 100644 src/test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4bc809a..124337e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,11 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation ) +find_package( + PkgConfig REQUIRED +) + +pkg_check_modules ( ncurses REQUIRED ncurses ) add_message_files( FILES @@ -25,6 +30,7 @@ catkin_package() include_directories( ${catkin_INCLUDE_DIRS} + ${CURSES_INCLUDE_DIRS} ) add_executable(filtre src/filtre.cpp) @@ -36,6 +42,11 @@ target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES}) add_executable(pcl_displayer src/pcl_displayer.cpp) target_link_libraries(pcl_displayer ${catkin_LIBRARIES}) +add_library(curses src/curses.cpp src/curses.h) + +add_executable(keyboard_cmd src/keyboard_cmd.cpp) +target_link_libraries(keyboard_cmd ${catkin_LIBRARIES} ${ncurses_LIBRARIES} curses) + add_executable(normal_estimator src/normal_estimator.cpp) target_link_libraries(normal_estimator ${catkin_LIBRARIES}) add_dependencies(normal_estimator hand_control_generate_messages_cpp) diff --git a/src/curses.cpp b/src/curses.cpp new file mode 100644 index 0000000..62f506f --- /dev/null +++ b/src/curses.cpp @@ -0,0 +1,37 @@ +#include "curses.h" +#include + +const int Curses::kbd_lines = 12; +const int Curses::kbd_columns = 50; +const int Curses::speed_lines = 4; +const int Curses::speed_columns = 50; + +Curses::Curses() { + initscr(); + cbreak(); + noecho(); + kbd = newwin(kbd_lines, kbd_columns, 0, 0); + kbd = newwin(speed_lines, speed_columns, + kbd_lines+1, 0); +} + +Curses::~Curses() { + delwin(kbd); + delwin(speed); + endwin(); +} + +void Curses::print_kbd() { + wmove(kbd, 0, 0); waddstr(kbd, " ---------------------"); + wmove(kbd, 1, 0); waddstr(kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); + wmove(kbd, 2, 0); waddstr(kbd, " |---|---|---|---|---|----"); + wmove(kbd, 3, 0); waddstr(kbd, " reset>| g|⇐ h|← j| k|→ l|⇒ m|"); + wmove(kbd, 4, 0); waddstr(kbd, " |---|---|---|---|---|----"); + wmove(kbd, 5, 0); waddstr(kbd, " land>| b|⇓ n|↙ ,|↓ ;|↘ :|"); + wmove(kbd, 6, 0); waddstr(kbd, " ---------------------"); + wmove(kbd, 8, 0); waddstr(kbd, "a/w : increase/decrease linear `x` speeds by 10%"); + wmove(kbd, 9, 0); waddstr(kbd, "z/x : increase/decrease linear `y` speed by 10%"); + wmove(kbd, 10, 0); waddstr(kbd, "e/c : increase/decrease linear `z` speed by 10%"); + wmove(kbd, 11, 0); waddstr(kbd, "r/v : increase/decrease rotation speed by 10%"); + wrefresh(kbd); +} diff --git a/src/curses.h b/src/curses.h new file mode 100644 index 0000000..559ce16 --- /dev/null +++ b/src/curses.h @@ -0,0 +1,23 @@ +#ifndef CURSES +#define CURSES + +#include + +class Curses +{ + private: + 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; + + public: + Curses(); + ~Curses(); + void print_kbd(); +}; + +#endif diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index ac52ac1..577ceb9 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include "curses.h" #include #include @@ -25,17 +25,14 @@ class Run void reset() { pub_reset.publish(empty); }; float x_speed, y_speed, z_speed, turn; - void print_speed() - { - std::cout << "x speed : " << x_speed << ", y speed : " << - y_speed << ", z speed : " << z_speed << - ", rotation speed : " << turn << "\n"; - }; + Curses term; + void print_speed() { ; }; public: Run() : n(), empty(), + term(), loop_rate(30), x_speed(0.2), y_speed(0.3), @@ -47,11 +44,6 @@ class Run pub_reset = n.advertise("/ardrone/reset", 1); } - ~Run() - { - endwin(); - } - void operator()() { @@ -71,10 +63,6 @@ class Run <<"r/v : increase/decrease rotation speed by 10%\n"; */ - initscr(); - cbreak(); - noecho(); - while (ros::ok()) { geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist()); @@ -232,12 +220,11 @@ class Run default : break; } // switch - ros::spinOnce(); loop_rate.sleep(); - } // while } // run + }; // class int main(int argc, char** argv) diff --git a/src/test.cpp b/src/test.cpp new file mode 100644 index 0000000..9c75f61 --- /dev/null +++ b/src/test.cpp @@ -0,0 +1,8 @@ +#include "curses.h" +int main() +{ +Curses term; +term.print_kbd(); +for(;;) ; +} + From e237bfd695da79009ae319ae1a1e7a447e6f0d25 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Fri, 1 May 2015 23:00:48 +0200 Subject: [PATCH 02/17] =?UTF-8?q?eur=C3=AAka=20!?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/curses.cpp | 6 ++++-- src/curses.h | 4 ++-- src/test.cpp | 2 ++ 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/curses.cpp b/src/curses.cpp index 62f506f..cc96524 100644 --- a/src/curses.cpp +++ b/src/curses.cpp @@ -1,5 +1,5 @@ -#include "curses.h" #include +#include "curses.h" const int Curses::kbd_lines = 12; const int Curses::kbd_columns = 50; @@ -11,7 +11,7 @@ Curses::Curses() { cbreak(); noecho(); kbd = newwin(kbd_lines, kbd_columns, 0, 0); - kbd = newwin(speed_lines, speed_columns, + speed = newwin(speed_lines, speed_columns, kbd_lines+1, 0); } @@ -22,6 +22,8 @@ Curses::~Curses() { } void Curses::print_kbd() { + wprintw(kbd,"%s", L"tuiiaueakeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); + wprintw(kbd,"%s", "tuiiaueakeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); wmove(kbd, 0, 0); waddstr(kbd, " ---------------------"); wmove(kbd, 1, 0); waddstr(kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); wmove(kbd, 2, 0); waddstr(kbd, " |---|---|---|---|---|----"); diff --git a/src/curses.h b/src/curses.h index 559ce16..5260925 100644 --- a/src/curses.h +++ b/src/curses.h @@ -1,5 +1,5 @@ -#ifndef CURSES -#define CURSES +#ifndef CURSES_TEST +#define CURSES_TEST #include diff --git a/src/test.cpp b/src/test.cpp index 9c75f61..6f18229 100644 --- a/src/test.cpp +++ b/src/test.cpp @@ -1,6 +1,8 @@ #include "curses.h" +#include "locale.h" int main() { +setlocale(LC_ALL, ""); Curses term; term.print_kbd(); for(;;) ; From b3226f33b962196aaefec68229e43a58987497c2 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Fri, 1 May 2015 23:22:13 +0200 Subject: [PATCH 03/17] node keyboard_cmd displays UTF-8 --- CMakeLists.txt | 8 ++++---- src/{curses.cpp => display.cpp} | 4 +--- src/{curses.h => display.h} | 0 src/keyboard_cmd.cpp | 31 +++++++------------------------ src/test.cpp | 2 +- 5 files changed, 13 insertions(+), 32 deletions(-) rename src/{curses.cpp => display.cpp} (89%) rename src/{curses.h => display.h} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 124337e..7fbe2d6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,7 @@ find_package( PkgConfig REQUIRED ) -pkg_check_modules ( ncurses REQUIRED ncurses ) +pkg_check_modules ( ncursesw REQUIRED ncursesw) add_message_files( FILES @@ -30,7 +30,7 @@ catkin_package() include_directories( ${catkin_INCLUDE_DIRS} - ${CURSES_INCLUDE_DIRS} + ${CURSESW_INCLUDE_DIRS} ) add_executable(filtre src/filtre.cpp) @@ -42,10 +42,10 @@ target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES}) add_executable(pcl_displayer src/pcl_displayer.cpp) target_link_libraries(pcl_displayer ${catkin_LIBRARIES}) -add_library(curses src/curses.cpp src/curses.h) +add_library(display src/display.cpp) add_executable(keyboard_cmd src/keyboard_cmd.cpp) -target_link_libraries(keyboard_cmd ${catkin_LIBRARIES} ${ncurses_LIBRARIES} curses) +target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRARIES}) add_executable(normal_estimator src/normal_estimator.cpp) target_link_libraries(normal_estimator ${catkin_LIBRARIES}) diff --git a/src/curses.cpp b/src/display.cpp similarity index 89% rename from src/curses.cpp rename to src/display.cpp index cc96524..d4f048f 100644 --- a/src/curses.cpp +++ b/src/display.cpp @@ -1,5 +1,5 @@ #include -#include "curses.h" +#include "display.h" const int Curses::kbd_lines = 12; const int Curses::kbd_columns = 50; @@ -22,8 +22,6 @@ Curses::~Curses() { } void Curses::print_kbd() { - wprintw(kbd,"%s", L"tuiiaueakeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); - wprintw(kbd,"%s", "tuiiaueakeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); wmove(kbd, 0, 0); waddstr(kbd, " ---------------------"); wmove(kbd, 1, 0); waddstr(kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); wmove(kbd, 2, 0); waddstr(kbd, " |---|---|---|---|---|----"); diff --git a/src/curses.h b/src/display.h similarity index 100% rename from src/curses.h rename to src/display.h diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index 577ceb9..a3805d0 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -5,8 +5,9 @@ #include #include #include - -#include "curses.h" +#include +#include +#include "display.h" #include #include @@ -30,9 +31,6 @@ class Run public: Run() : - n(), - empty(), - term(), loop_rate(30), x_speed(0.2), y_speed(0.3), @@ -46,23 +44,6 @@ class Run void operator()() { - - /* - std::cout - <<" ---------------------\n" - <<"takeoff>| t|⇑ y|↖ u|↑ i|↗ o|\n" - <<" |---|---|---|---|---|----\n" - <<" reset>| g|⇐ h|← j| k|→ l|⇒ m|\n" - <<" |---|---|---|---|---|----\n" - <<" land>| b|⇓ n|↙ ,|↓ ;|↘ :|\n" - <<" ---------------------\n" - <<"\n" - <<"a/w : increase/decrease linear `x` speeds by 10%\n" - <<"z/x : increase/decrease linear `y` speed by 10%\n" - <<"e/c : increase/decrease linear `z` speed by 10%\n" - <<"r/v : increase/decrease rotation speed by 10%\n"; - */ - while (ros::ok()) { geometry_msgs::Twist::Ptr msg(new geometry_msgs::Twist()); @@ -229,9 +210,11 @@ class Run int main(int argc, char** argv) { + setlocale(LC_ALL, ""); ros::init(argc, argv, "keyboard_cmd"); - Run run; - run(); + Curses terminal; + terminal.print_kbd(); + for(;;) ; return 0; } diff --git a/src/test.cpp b/src/test.cpp index 6f18229..bfe933b 100644 --- a/src/test.cpp +++ b/src/test.cpp @@ -1,4 +1,4 @@ -#include "curses.h" +#include "display.h" #include "locale.h" int main() { From f09c762cb23df59d1dac07dc885e509144803aad Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 00:00:28 +0200 Subject: [PATCH 04/17] corrects how to get the char --- src/display.cpp | 15 ++++++++++++--- src/display.h | 4 +++- src/keyboard_cmd.cpp | 13 +++++++------ 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/display.cpp b/src/display.cpp index d4f048f..dab99c8 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -9,18 +9,27 @@ const int Curses::speed_columns = 50; Curses::Curses() { initscr(); cbreak(); - noecho(); + //noecho(); kbd = newwin(kbd_lines, kbd_columns, 0, 0); - speed = newwin(speed_lines, speed_columns, - kbd_lines+1, 0); + speed = newwin(speed_lines, speed_columns, kbd_lines+1, 0); + get = newwin(1,1,kbd_lines+speed_lines+1,2); + print_kbd(); + wmove(get, 0, 0); + wrefresh(get); } Curses::~Curses() { delwin(kbd); delwin(speed); + delwin(get); endwin(); } +char Curses::getchar() +{ + return wgetch(get); +} + void Curses::print_kbd() { wmove(kbd, 0, 0); waddstr(kbd, " ---------------------"); wmove(kbd, 1, 0); waddstr(kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); diff --git a/src/display.h b/src/display.h index 5260925..341c318 100644 --- a/src/display.h +++ b/src/display.h @@ -13,11 +13,13 @@ class Curses static const int speed_lines; static const int speed_columns; WINDOW* speed; + void print_kbd(); + WINDOW* get; public: Curses(); ~Curses(); - void print_kbd(); + char getchar(); }; #endif diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index a3805d0..31f118e 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -26,11 +26,12 @@ class Run void reset() { pub_reset.publish(empty); }; float x_speed, y_speed, z_speed, turn; - Curses term; + boost::shared_ptr term; void print_speed() { ; }; public: - Run() : + Run(boost::shared_ptr terminal) : + term(terminal), loop_rate(30), x_speed(0.2), y_speed(0.3), @@ -50,7 +51,7 @@ class Run msg->linear.x = msg->linear.y = msg->linear.z = msg->angular.x = msg->angular.y = msg->angular.z = 0.; - char c = getch(); + char c = term->getchar(); switch(c) { @@ -212,9 +213,9 @@ int main(int argc, char** argv) { setlocale(LC_ALL, ""); ros::init(argc, argv, "keyboard_cmd"); - Curses terminal; - terminal.print_kbd(); - for(;;) ; + boost::shared_ptr term(new Curses()); + Run fun(term); + fun(); return 0; } From 6eac98cab3f8f3b773761c66cfa98fc55688b4da Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 00:40:28 +0200 Subject: [PATCH 05/17] keyboard_cmd on progress --- src/display.h | 14 ++++++++--- src/keyboard_cmd.cpp | 56 +++++++++++++++++++++++++++++--------------- 2 files changed, 48 insertions(+), 22 deletions(-) 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 From 2e325bbf96fe082bc94f30d9ba4e9894aa96d331 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 00:57:33 +0200 Subject: [PATCH 06/17] rm test.cpp --- src/test.cpp | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100644 src/test.cpp diff --git a/src/test.cpp b/src/test.cpp deleted file mode 100644 index bfe933b..0000000 --- a/src/test.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "display.h" -#include "locale.h" -int main() -{ -setlocale(LC_ALL, ""); -Curses term; -term.print_kbd(); -for(;;) ; -} - From df80fbfcb867d25630dc1d9db30363102fb11b64 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 15:37:58 +0200 Subject: [PATCH 07/17] on progress --- src/display.cpp | 74 +++++++++++++++++++++++++++++--------------- src/display.h | 24 ++++++++------ src/keyboard_cmd.cpp | 52 ++++++++++++++++++------------- 3 files changed, 94 insertions(+), 56 deletions(-) diff --git a/src/display.cpp b/src/display.cpp index dab99c8..7c96091 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -1,46 +1,70 @@ #include +#include #include "display.h" -const int Curses::kbd_lines = 12; -const int Curses::kbd_columns = 50; -const int Curses::speed_lines = 4; -const int Curses::speed_columns = 50; +const int Curses::cmd_kbd_lines = 12; +const int Curses::cmd_kbd_columns = 50; +const int Curses::cmd_speed_lines = 4; +const int Curses::cmd_speed_columns = 50; Curses::Curses() { initscr(); cbreak(); //noecho(); - kbd = newwin(kbd_lines, kbd_columns, 0, 0); - speed = newwin(speed_lines, speed_columns, kbd_lines+1, 0); - get = newwin(1,1,kbd_lines+speed_lines+1,2); - print_kbd(); + cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0); + cmd_speed = newwin(cmd_speed_lines, + cmd_speed_columns, cmd_kbd_lines+1, 0); + get = newwin(1,1,cmd_kbd_lines + cmd_speed_lines + 1,2); + //log_sent_w = newwin(/// + //nav_data = newwin(// + print_cmd_kbd(); + print_cmd_speed(); wmove(get, 0, 0); wrefresh(get); } Curses::~Curses() { - delwin(kbd); - delwin(speed); + delwin(cmd_kbd); + delwin(cmd_speed); + delwin(log_sent_w); + delwin(nav_data); delwin(get); endwin(); } -char Curses::getchar() -{ +char Curses::getchar() { return wgetch(get); } -void Curses::print_kbd() { - wmove(kbd, 0, 0); waddstr(kbd, " ---------------------"); - wmove(kbd, 1, 0); waddstr(kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); - wmove(kbd, 2, 0); waddstr(kbd, " |---|---|---|---|---|----"); - wmove(kbd, 3, 0); waddstr(kbd, " reset>| g|⇐ h|← j| k|→ l|⇒ m|"); - wmove(kbd, 4, 0); waddstr(kbd, " |---|---|---|---|---|----"); - wmove(kbd, 5, 0); waddstr(kbd, " land>| b|⇓ n|↙ ,|↓ ;|↘ :|"); - wmove(kbd, 6, 0); waddstr(kbd, " ---------------------"); - wmove(kbd, 8, 0); waddstr(kbd, "a/w : increase/decrease linear `x` speeds by 10%"); - wmove(kbd, 9, 0); waddstr(kbd, "z/x : increase/decrease linear `y` speed by 10%"); - wmove(kbd, 10, 0); waddstr(kbd, "e/c : increase/decrease linear `z` speed by 10%"); - wmove(kbd, 11, 0); waddstr(kbd, "r/v : increase/decrease rotation speed by 10%"); - wrefresh(kbd); +void Curses::print_cmd_kbd() { + wmove(cmd_kbd, 0, 0); waddstr(cmd_kbd, " ---------------------"); + wmove(cmd_kbd, 1, 0); waddstr(cmd_kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); + wmove(cmd_kbd, 2, 0); waddstr(cmd_kbd, " |---|---|---|---|---|----"); + wmove(cmd_kbd, 3, 0); waddstr(cmd_kbd, " reset>| g|⇐ h|← j| k|→ l|⇒ m|"); + wmove(cmd_kbd, 4, 0); waddstr(cmd_kbd, " |---|---|---|---|---|----"); + wmove(cmd_kbd, 5, 0); waddstr(cmd_kbd, " land>| b|⇓ n|↙ ,|↓ ;|↘ :|"); + wmove(cmd_kbd, 6, 0); waddstr(cmd_kbd, " ---------------------"); + wmove(cmd_kbd, 8, 0); waddstr(cmd_kbd, "a/w : increase/decrease linear `x` speeds by 10%"); + wmove(cmd_kbd, 9, 0); waddstr(cmd_kbd, "z/x : increase/decrease linear `y` speed by 10%"); + wmove(cmd_kbd, 10, 0); waddstr(cmd_kbd, "e/c : increase/decrease linear `z` speed by 10%"); + wmove(cmd_kbd, 11, 0); waddstr(cmd_kbd, "r/v : increase/decrease rotation speed by 10%"); + wrefresh(cmd_kbd); +} + +void Curses::print_cmd_speed() { + +} + +void Curses::update_cmd_speed(const char& coord, const float& v) { + +} + +void Curses::log_sent(const std::string& str) { + +} + +void Curses::update_navdata(const float& batteryPercent, + const int& state, + const float& time) { + } diff --git a/src/display.h b/src/display.h index 5e6edd7..367f10f 100644 --- a/src/display.h +++ b/src/display.h @@ -2,17 +2,19 @@ #define CURSES_DISPLAY #include +#include class Curses { private: - static const int kbd_lines; - static const int kbd_columns; - WINDOW* kbd; - static const int speed_lines; - static const int speed_columns; - WINDOW* speed; - void print_kbd(); + static const int cmd_kbd_lines; + static const int cmd_kbd_columns; + WINDOW* cmd_kbd; + static const int cmd_speed_lines; + static const int cmd_speed_columns; + WINDOW* cmd_speed; + void print_cmd_kbd(); + void print_cmd_speed(); WINDOW* get; // TODO (avec scroll) @@ -25,9 +27,11 @@ class Curses char getchar(); // TODO - void update_speed(const char&, const float&); - // void update_navdata(... - void log_sent(char*); + void update_cmd_speed(const char& coord, const float& v); + void update_navdata(const float& batteryPercent, + const int& state, + const float& time); + void log_sent(const std::string& str); }; #endif diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index a523d33..f151fff 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -6,6 +6,21 @@ #include #include +#include + +class Callback +{ + private: + boost::shared_ptr term; + + public: + Callback(const boost::shared_ptr& terminal) : + term(terminal) {} + + void operator()(const ardrone_autonomy::Navdata::ConstPtr msg) { + term->update_navdata(msg->batteryPercent, msg->state, msg->tm); + } +}; // class Callback class Run { @@ -14,17 +29,17 @@ class Run ros::NodeHandle n; ros::Rate loop_rate; ros::Publisher cmd, pub_takeoff, pub_land, pub_reset; - void land() - { pub_land.publish(empty); }; - void takeoff() - { pub_takeoff.publish(empty); }; - void reset() - { pub_reset.publish(empty); }; + ros::Subscriber data_subscriber; + void land() { pub_land.publish(empty); }; + void takeoff() { pub_takeoff.publish(empty); }; + void reset() { pub_reset.publish(empty); }; float x_speed, y_speed, z_speed, turn; boost::shared_ptr term; + Callback data_callback; public: - Run(boost::shared_ptr terminal) : + Run(const boost::shared_ptr& terminal) : + data_callback(terminal), term(terminal), loop_rate(30), x_speed(0.2), @@ -35,6 +50,7 @@ class Run pub_takeoff = n.advertise("/ardrone/takeoff", 1); pub_land = n.advertise("/ardrone/land", 1); pub_reset = n.advertise("/ardrone/reset", 1); + data_subscriber = n.subscribe("/ardrone/navdata", 1, data_callback); } void operator()() @@ -166,49 +182,49 @@ class Run case 'a' : {// + x_speed x_speed *= 1.1; - term->update_speed('x', x_speed); + term->update_cmd_speed('x', x_speed); break; } case 'w' : {// - x_speed x_speed *= 0.9; - term->update_speed('x', x_speed); + term->update_cmd_speed('x', x_speed); break; } case 'z' : {// + y_speed y_speed *= 1.1; - term->update_speed('y', y_speed); + term->update_cmd_speed('y', y_speed); break; } case 'x' : {// - y_speed y_speed *= 0.9; - term->update_speed('y', y_speed); + term->update_cmd_speed('y', y_speed); break; } case 'e' : {// + z_speed z_speed *= 1.1; - term->update_speed('z', z_speed); + term->update_cmd_speed('z', z_speed); break; } case 'c' : {// - z_speed z_speed *= 0.9; - term->update_speed('z', z_speed); + term->update_cmd_speed('z', z_speed); break; } case 'r' : {// + turn speed turn *= 1.1; - term->update_speed('t', turn); + term->update_cmd_speed('t', turn); break; } case 'v' : {// - turn speed turn *= 0.9; - term->update_speed('t', turn); + term->update_cmd_speed('t', turn); break; } default : @@ -220,12 +236,6 @@ class Run }; // 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, ""); From 08f4e835ab84fc4b176d35da7fb2f0cbeaf76e0c Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 15:48:40 +0200 Subject: [PATCH 08/17] not enough progress --- CMakeLists.txt | 3 ++- src/keyboard_cmd.cpp | 3 +-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7fbe2d6..9dcede5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs message_generation + ardrone_autonomy ) find_package( PkgConfig REQUIRED @@ -30,7 +31,7 @@ catkin_package() include_directories( ${catkin_INCLUDE_DIRS} - ${CURSESW_INCLUDE_DIRS} + ${ncursesw_INCLUDE_DIRS} ) add_executable(filtre src/filtre.cpp) diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index f151fff..4a35a37 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -17,7 +17,7 @@ class Callback Callback(const boost::shared_ptr& terminal) : term(terminal) {} - void operator()(const ardrone_autonomy::Navdata::ConstPtr msg) { + void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) { term->update_navdata(msg->batteryPercent, msg->state, msg->tm); } }; // class Callback @@ -240,7 +240,6 @@ 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(); From e70baf354f5fb25cd4b81c70a3dacf2db043472a Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 18:02:58 +0200 Subject: [PATCH 09/17] adds Header and timing to estimated plans --- CMakeLists.txt | 4 +++- msg/Plan.msg | 1 + src/normal_estimator.cpp | 13 +++++++++++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9dcede5..c8c9ac3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,9 @@ generate_messages( geometry_msgs ) -catkin_package() +catkin_package( +CATKIN_DEPENDS message_runtime +) include_directories( ${catkin_INCLUDE_DIRS} diff --git a/msg/Plan.msg b/msg/Plan.msg index 9b30a24..1c8921a 100644 --- a/msg/Plan.msg +++ b/msg/Plan.msg @@ -1,3 +1,4 @@ +Header header geometry_msgs/Point normal float64 altitude float64 curvature diff --git a/src/normal_estimator.cpp b/src/normal_estimator.cpp index 26dd806..5617e04 100644 --- a/src/normal_estimator.cpp +++ b/src/normal_estimator.cpp @@ -3,6 +3,7 @@ #include #include #include +#include typedef pcl::PointXYZRGB Point; typedef pcl::PointCloud PointCloud; @@ -28,7 +29,7 @@ class Callback { // publication ROS_INFO("Plan published"); - publisher.publish(to_Plan(x, y, z, h, c)); + publisher.publish(to_Plan(x, y, z, h, c, msg->header.stamp)); } Callback(ros::Publisher& pub) : publisher(pub), estimator() {} @@ -36,12 +37,13 @@ class Callback { private: ros::Publisher publisher; pcl::NormalEstimationOMP estimator; + uint32_t number; inline const hand_control::Plan::ConstPtr to_Plan(const float& x, const float& y, const float& z, const float& h, - const float& c) + const float& c, uint64_t msec64) { hand_control::Plan::Ptr ros_msg(new hand_control::Plan()); ros_msg->normal.x = x; @@ -49,6 +51,13 @@ class Callback { ros_msg->normal.z = z; ros_msg->altitude = h; ros_msg->curvature = c; + // uint64_t msec64 is in ms (10-6) + uint64_t sec64 = msec64 / 1000000; + uint64_t nsec64 = (msec64 % 1000000) * 1000; + ros_msg->header.stamp.sec = (uint32_t) sec64; + ros_msg->header.stamp.nsec = (uint32_t) nsec64; + ros_msg->header.seq = number++; + ros_msg->header.frame_id = "0"; return ros_msg; } From cae0e41b7c575a6b273fbcda76eeab676aec831f Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 18:04:56 +0200 Subject: [PATCH 10/17] corrects plans numbering --- src/normal_estimator.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/normal_estimator.cpp b/src/normal_estimator.cpp index 5617e04..ac433c5 100644 --- a/src/normal_estimator.cpp +++ b/src/normal_estimator.cpp @@ -32,7 +32,8 @@ class Callback { publisher.publish(to_Plan(x, y, z, h, c, msg->header.stamp)); } - Callback(ros::Publisher& pub) : publisher(pub), estimator() {} + Callback(ros::Publisher& pub) : + publisher(pub), estimator(), number(0) {} private: ros::Publisher publisher; From 454b0acd65a243a4e5a0b0cfe72f1c133f9a46d6 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 18:28:40 +0200 Subject: [PATCH 11/17] better numbering in normal_estimator --- src/normal_estimator.cpp | 9 ++++----- src/random_pcl_publisher.cpp | 8 +++++++- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/normal_estimator.cpp b/src/normal_estimator.cpp index ac433c5..8a30d83 100644 --- a/src/normal_estimator.cpp +++ b/src/normal_estimator.cpp @@ -29,22 +29,21 @@ class Callback { // publication ROS_INFO("Plan published"); - publisher.publish(to_Plan(x, y, z, h, c, msg->header.stamp)); + publisher.publish(to_Plan(x, y, z, h, c, msg->header.seq, msg->header.stamp)); } Callback(ros::Publisher& pub) : - publisher(pub), estimator(), number(0) {} + publisher(pub), estimator() {} private: ros::Publisher publisher; pcl::NormalEstimationOMP estimator; - uint32_t number; inline const hand_control::Plan::ConstPtr to_Plan(const float& x, const float& y, const float& z, const float& h, - const float& c, uint64_t msec64) + const float& c, uint32_t seq, uint64_t msec64) { hand_control::Plan::Ptr ros_msg(new hand_control::Plan()); ros_msg->normal.x = x; @@ -57,7 +56,7 @@ class Callback { uint64_t nsec64 = (msec64 % 1000000) * 1000; ros_msg->header.stamp.sec = (uint32_t) sec64; ros_msg->header.stamp.nsec = (uint32_t) nsec64; - ros_msg->header.seq = number++; + ros_msg->header.seq = seq; ros_msg->header.frame_id = "0"; return ros_msg; } diff --git a/src/random_pcl_publisher.cpp b/src/random_pcl_publisher.cpp index 63c2acd..394f0d9 100644 --- a/src/random_pcl_publisher.cpp +++ b/src/random_pcl_publisher.cpp @@ -1,4 +1,5 @@ #include +#include #include #include // for UniformGenerator @@ -13,7 +14,7 @@ class Generator { public: Generator(int len, double m, double M) - : length(len), min(m), max(M), cgen() + : length(len), min(m), max(M), cgen(), number(0) { UGenerator::Parameters params(min, max, -1); cgen.setParameters(params); @@ -30,6 +31,10 @@ class Generator pcl->points[i].g = (uint8_t) 255; pcl->points[i].b = (uint8_t) 0; } + ros::Time now = ros::Time::now(); + pcl->header.stamp = now.toNSec() / 1000; + pcl->header.seq = number++; + pcl->header.frame_id = "0"; return pcl; } @@ -37,6 +42,7 @@ class Generator pcl::common::CloudGenerator cgen; int length; double min, max; + uint32_t number; }; int From f211313cd5371b35ac24fc4f7dbe179f5d8cefd6 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 19:24:54 +0200 Subject: [PATCH 12/17] adds sent commands log --- src/display.cpp | 37 +++++++++++++++++++++++++++++++------ src/display.h | 13 +++++++++++-- 2 files changed, 42 insertions(+), 8 deletions(-) diff --git a/src/display.cpp b/src/display.cpp index 7c96091..c9bb21d 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -4,21 +4,44 @@ const int Curses::cmd_kbd_lines = 12; const int Curses::cmd_kbd_columns = 50; + const int Curses::cmd_speed_lines = 4; const int Curses::cmd_speed_columns = 50; +const int Curses::get_lines = 1; +const int Curses::get_columns = 1; + +const int Curses::nav_data_lines = 5; +const int Curses::nav_data_columns = 40; + +const int Curses::log_sent_w_lines = 12; +const int Curses::log_sent_w_columns = 40; + Curses::Curses() { initscr(); cbreak(); //noecho(); + cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0); - cmd_speed = newwin(cmd_speed_lines, - cmd_speed_columns, cmd_kbd_lines+1, 0); - get = newwin(1,1,cmd_kbd_lines + cmd_speed_lines + 1,2); - //log_sent_w = newwin(/// - //nav_data = newwin(// + + get = newwin(get_lines, get_columns, + cmd_kbd_lines, cmd_kbd_columns/2); + + cmd_speed = newwin(cmd_speed_lines, cmd_speed_columns, + cmd_kbd_lines + get_lines, 0); + + log_sent_w = newwin(log_sent_w_lines, log_sent_w_columns, + 0, cmd_kbd_columns + 1); + log_line_number = log_sent_w_lines - 1; + + scrollok(log_sent_w, TRUE); + + nav_data = newwin(nav_data_lines, nav_data_columns, + log_sent_w_lines + 1, cmd_kbd_columns + 1); + print_cmd_kbd(); print_cmd_speed(); + wmove(get, 0, 0); wrefresh(get); } @@ -60,7 +83,9 @@ void Curses::update_cmd_speed(const char& coord, const float& v) { } void Curses::log_sent(const std::string& str) { - + wmove(log_sent_w, log_line_number++, 0); + waddstr(log_sent_w, (str + "\n").c_str() ); + wrefresh(log_sent_w); } void Curses::update_navdata(const float& batteryPercent, diff --git a/src/display.h b/src/display.h index 367f10f..07127e5 100644 --- a/src/display.h +++ b/src/display.h @@ -10,15 +10,24 @@ class Curses static const int cmd_kbd_lines; static const int cmd_kbd_columns; WINDOW* cmd_kbd; + void print_cmd_kbd(); + static const int cmd_speed_lines; static const int cmd_speed_columns; WINDOW* cmd_speed; - void print_cmd_kbd(); void print_cmd_speed(); + + static const int get_lines; + static const int get_columns; WINDOW* get; - // TODO (avec scroll) + static const int log_sent_w_lines; + static const int log_sent_w_columns; + int log_line_number; WINDOW* log_sent_w; + + static const int nav_data_lines; + static const int nav_data_columns; WINDOW* nav_data; public: From 5fcf47d77a3ef4437302906223ea4b191ff8244c Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 19:46:02 +0200 Subject: [PATCH 13/17] adds colors to log_sent_w --- src/display.cpp | 4 ++++ src/display.h | 1 + 2 files changed, 5 insertions(+) diff --git a/src/display.cpp b/src/display.cpp index c9bb21d..ca3f200 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -33,6 +33,10 @@ Curses::Curses() { log_sent_w = newwin(log_sent_w_lines, log_sent_w_columns, 0, cmd_kbd_columns + 1); log_line_number = log_sent_w_lines - 1; + wattron(log_sent_w, A_BOLD); + start_color(); + init_pair(1, COLOR_RED, COLOR_BLACK); + wattron(log_sent_w, COLOR_PAIR(1)); scrollok(log_sent_w, TRUE); diff --git a/src/display.h b/src/display.h index 07127e5..26b2891 100644 --- a/src/display.h +++ b/src/display.h @@ -25,6 +25,7 @@ class Curses static const int log_sent_w_columns; int log_line_number; WINDOW* log_sent_w; + WINDOW* log_sent_title; static const int nav_data_lines; static const int nav_data_columns; From 743c4fafd0fc755be140f1975d72eb58aeb45191 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 19:58:04 +0200 Subject: [PATCH 14/17] adds title to sent commands log --- src/display.cpp | 14 +++++++++----- src/keyboard_cmd.cpp | 4 ++++ 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/display.cpp b/src/display.cpp index ca3f200..91736eb 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -20,6 +20,7 @@ const int Curses::log_sent_w_columns = 40; Curses::Curses() { initscr(); cbreak(); + start_color(); //noecho(); cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0); @@ -30,14 +31,16 @@ Curses::Curses() { cmd_speed = newwin(cmd_speed_lines, cmd_speed_columns, cmd_kbd_lines + get_lines, 0); - log_sent_w = newwin(log_sent_w_lines, log_sent_w_columns, - 0, cmd_kbd_columns + 1); - log_line_number = log_sent_w_lines - 1; + log_sent_title = newwin(1, log_sent_w_columns, + 0, cmd_kbd_columns + 1); + waddstr(log_sent_title, "SENT COMMANDS"); + wrefresh(log_sent_title); + log_sent_w = newwin(log_sent_w_lines - 1, log_sent_w_columns, + 1, cmd_kbd_columns + 1); + log_line_number = log_sent_w_lines - 2; wattron(log_sent_w, A_BOLD); - start_color(); init_pair(1, COLOR_RED, COLOR_BLACK); wattron(log_sent_w, COLOR_PAIR(1)); - scrollok(log_sent_w, TRUE); nav_data = newwin(nav_data_lines, nav_data_columns, @@ -54,6 +57,7 @@ Curses::~Curses() { delwin(cmd_kbd); delwin(cmd_speed); delwin(log_sent_w); + delwin(log_sent_title); delwin(nav_data); delwin(get); endwin(); diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index 4a35a37..1dc51cb 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -228,6 +228,10 @@ class Run break; } default : + { + cmd.publish(msg); + term->log_sent("hover !"); + } break; } // switch(c) loop_rate.sleep(); From 4c4d3b11aa1358823443414792c8d27078fe57ed Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 20:52:46 +0200 Subject: [PATCH 15/17] keyboard_cmd displays cmd speeds --- src/display.cpp | 69 +++++++++++++++++++++++++++++++------------- src/display.h | 1 + src/keyboard_cmd.cpp | 6 +++- 3 files changed, 55 insertions(+), 21 deletions(-) diff --git a/src/display.cpp b/src/display.cpp index 91736eb..7171c9a 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -2,17 +2,17 @@ #include #include "display.h" -const int Curses::cmd_kbd_lines = 12; -const int Curses::cmd_kbd_columns = 50; +const int Curses::cmd_kbd_lines = 7; +const int Curses::cmd_kbd_columns = 55; const int Curses::cmd_speed_lines = 4; -const int Curses::cmd_speed_columns = 50; +const int Curses::cmd_speed_columns = 55; const int Curses::get_lines = 1; const int Curses::get_columns = 1; -const int Curses::nav_data_lines = 5; -const int Curses::nav_data_columns = 40; +const int Curses::nav_data_lines = 3; +const int Curses::nav_data_columns = 55; const int Curses::log_sent_w_lines = 12; const int Curses::log_sent_w_columns = 40; @@ -44,8 +44,9 @@ Curses::Curses() { scrollok(log_sent_w, TRUE); nav_data = newwin(nav_data_lines, nav_data_columns, - log_sent_w_lines + 1, cmd_kbd_columns + 1); + cmd_kbd_lines + get_lines + cmd_speed_lines + 1, 0); + print_nav_data(); print_cmd_kbd(); print_cmd_speed(); @@ -68,26 +69,48 @@ char Curses::getchar() { } void Curses::print_cmd_kbd() { - wmove(cmd_kbd, 0, 0); waddstr(cmd_kbd, " ---------------------"); - wmove(cmd_kbd, 1, 0); waddstr(cmd_kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|"); - wmove(cmd_kbd, 2, 0); waddstr(cmd_kbd, " |---|---|---|---|---|----"); - wmove(cmd_kbd, 3, 0); waddstr(cmd_kbd, " reset>| g|⇐ h|← j| k|→ l|⇒ m|"); - wmove(cmd_kbd, 4, 0); waddstr(cmd_kbd, " |---|---|---|---|---|----"); - wmove(cmd_kbd, 5, 0); waddstr(cmd_kbd, " land>| b|⇓ n|↙ ,|↓ ;|↘ :|"); - wmove(cmd_kbd, 6, 0); waddstr(cmd_kbd, " ---------------------"); - wmove(cmd_kbd, 8, 0); waddstr(cmd_kbd, "a/w : increase/decrease linear `x` speeds by 10%"); - wmove(cmd_kbd, 9, 0); waddstr(cmd_kbd, "z/x : increase/decrease linear `y` speed by 10%"); - wmove(cmd_kbd, 10, 0); waddstr(cmd_kbd, "e/c : increase/decrease linear `z` speed by 10%"); - wmove(cmd_kbd, 11, 0); waddstr(cmd_kbd, "r/v : increase/decrease rotation speed by 10%"); + wmove(cmd_kbd, 0, 0); + waddstr(cmd_kbd, " ---------------------\n"); + waddstr(cmd_kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|\n"); + waddstr(cmd_kbd, " |---|---|---|---|---|----\n"); + waddstr(cmd_kbd, " reset>| g|⇐ h|← j| k|→ l|⇒ m|\n"); + waddstr(cmd_kbd, " |---|---|---|---|---|----\n"); + waddstr(cmd_kbd, " land>| b|⇓ n|↙ ,|↓ ;|↘ :|\n"); + waddstr(cmd_kbd, " ---------------------\n"); wrefresh(cmd_kbd); } void Curses::print_cmd_speed() { - + wmove(cmd_speed, 0, 0); + waddstr(cmd_speed, " `x` cmd speed : (a/w : increase/decrease)\n"); + waddstr(cmd_speed, " `y` cmd speed : (z/x : increase/decrease)\n"); + waddstr(cmd_speed, " `z` cmd speed : (e/c : increase/decrease)\n"); + waddstr(cmd_speed, "rotation speed : (r/v : increase/decrease)\n"); + wrefresh(cmd_speed); } void Curses::update_cmd_speed(const char& coord, const float& v) { - + switch(coord) { + case 'x': + wmove(cmd_speed, 0, 16); + wprintw(cmd_speed, "%f", v); + break; + case 'y': + wmove(cmd_speed, 1, 16); + wprintw(cmd_speed, "%f", v); + break; + case 'z': + wmove(cmd_speed, 2, 16); + wprintw(cmd_speed, "%f", v); + break; + case 't': + wmove(cmd_speed, 3, 16); + wprintw(cmd_speed, "%f", v); + break; + default: + ; + } + wrefresh(cmd_speed); } void Curses::log_sent(const std::string& str) { @@ -96,8 +119,14 @@ void Curses::log_sent(const std::string& str) { wrefresh(log_sent_w); } + +void Curses::print_nav_data() { + wmove(nav_data, 0, 0); + waddstr(nav_data, "Battery :\n State :\n Time :"); + wrefresh(nav_data); +} + void Curses::update_navdata(const float& batteryPercent, const int& state, const float& time) { - } diff --git a/src/display.h b/src/display.h index 26b2891..7413d10 100644 --- a/src/display.h +++ b/src/display.h @@ -30,6 +30,7 @@ class Curses static const int nav_data_lines; static const int nav_data_columns; WINDOW* nav_data; + void print_nav_data(); public: Curses(); diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index 1dc51cb..e1102ce 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -51,6 +51,11 @@ class Run pub_land = n.advertise("/ardrone/land", 1); pub_reset = n.advertise("/ardrone/reset", 1); data_subscriber = n.subscribe("/ardrone/navdata", 1, data_callback); + + term->update_cmd_speed('x', x_speed); + term->update_cmd_speed('y', y_speed); + term->update_cmd_speed('z', z_speed); + term->update_cmd_speed('t', turn); } void operator()() @@ -232,7 +237,6 @@ class Run cmd.publish(msg); term->log_sent("hover !"); } - break; } // switch(c) loop_rate.sleep(); } // while From bd757b23eacf74c90b7e79dba9bf55cdca44585c Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 21:18:47 +0200 Subject: [PATCH 16/17] adds nav_date displayer to keyboard_cmd --- src/display.cpp | 64 +++++++++++++++++++++++++++++++++++++------- src/display.h | 2 +- src/keyboard_cmd.cpp | 7 ++++- 3 files changed, 61 insertions(+), 12 deletions(-) diff --git a/src/display.cpp b/src/display.cpp index 7171c9a..333a258 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -2,7 +2,7 @@ #include #include "display.h" -const int Curses::cmd_kbd_lines = 7; +const int Curses::cmd_kbd_lines = 8; const int Curses::cmd_kbd_columns = 55; const int Curses::cmd_speed_lines = 4; @@ -39,12 +39,15 @@ Curses::Curses() { 1, cmd_kbd_columns + 1); log_line_number = log_sent_w_lines - 2; wattron(log_sent_w, A_BOLD); - init_pair(1, COLOR_RED, COLOR_BLACK); + init_pair(1, COLOR_GREEN, COLOR_BLACK); wattron(log_sent_w, COLOR_PAIR(1)); scrollok(log_sent_w, TRUE); nav_data = newwin(nav_data_lines, nav_data_columns, cmd_kbd_lines + get_lines + cmd_speed_lines + 1, 0); + init_pair(2, COLOR_RED, COLOR_BLACK); + wattron(nav_data, COLOR_PAIR(2)); + wattron(nav_data, A_BOLD); print_nav_data(); print_cmd_kbd(); @@ -70,13 +73,14 @@ char Curses::getchar() { void Curses::print_cmd_kbd() { wmove(cmd_kbd, 0, 0); - waddstr(cmd_kbd, " ---------------------\n"); - waddstr(cmd_kbd, "takeoff>| t|⇑ y|↖ u|↑ i|↗ o|\n"); - waddstr(cmd_kbd, " |---|---|---|---|---|----\n"); - waddstr(cmd_kbd, " reset>| g|⇐ h|← j| k|→ l|⇒ m|\n"); - waddstr(cmd_kbd, " |---|---|---|---|---|----\n"); - waddstr(cmd_kbd, " land>| b|⇓ n|↙ ,|↓ ;|↘ :|\n"); - waddstr(cmd_kbd, " ---------------------\n"); + waddstr(cmd_kbd, " ---------------------\n"); + waddstr(cmd_kbd, " takeoff>| t|⇑ y|↖ u|↑ i|↗ o|\n"); + waddstr(cmd_kbd, " |---|---|---|---|---|----\n"); + waddstr(cmd_kbd, " reset>| g|⇐ h|← j| k|→ l|⇒ m|\n"); + waddstr(cmd_kbd, " |---|---|---|---|---|----\n"); + waddstr(cmd_kbd, " land>| b|⇓ n|↙ ,|↓ ;|↘ :|\n"); + waddstr(cmd_kbd, " ---------------------\n"); + waddstr(cmd_kbd, " Press ctrl + C to quit"); wrefresh(cmd_kbd); } @@ -126,7 +130,47 @@ void Curses::print_nav_data() { wrefresh(nav_data); } -void Curses::update_navdata(const float& batteryPercent, +void Curses::update_nav_data(const float& batteryPercent, const int& state, const float& time) { + wmove(nav_data, 0, 10); + wprintw(nav_data, "%f %", batteryPercent); + wmove(nav_data, 2, 10); + wprintw(nav_data, "%f %", time); + wmove(nav_data, 1, 10); + switch(state) { + case 0: + waddstr(nav_data, "unknown "); + break; + case 1: + waddstr(nav_data, "inited "); + break; + case 2: + waddstr(nav_data, "landed "); + break; + case 3: + waddstr(nav_data, "flying "); + break; + case 4: + waddstr(nav_data, "hovering "); + break; + case 5: + waddstr(nav_data, "test "); + break; + case 6: + waddstr(nav_data, "taking off "); + break; + case 7: + waddstr(nav_data, "flying "); + break; + case 8: + waddstr(nav_data, "landing "); + break; + case 9: + waddstr(nav_data, "looping "); + break; + default: + ; + } + wrefresh(nav_data); } diff --git a/src/display.h b/src/display.h index 7413d10..5afc6e7 100644 --- a/src/display.h +++ b/src/display.h @@ -39,7 +39,7 @@ class Curses // TODO void update_cmd_speed(const char& coord, const float& v); - void update_navdata(const float& batteryPercent, + void update_nav_data(const float& batteryPercent, const int& state, const float& time); void log_sent(const std::string& str); diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index e1102ce..e219e5a 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -18,7 +18,7 @@ class Callback term(terminal) {} void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) { - term->update_navdata(msg->batteryPercent, msg->state, msg->tm); + term->update_nav_data(msg->batteryPercent, msg->state, msg->tm); } }; // class Callback @@ -56,6 +56,11 @@ class Run term->update_cmd_speed('y', y_speed); term->update_cmd_speed('z', z_speed); term->update_cmd_speed('t', turn); + + float a(0); + int s(0); + float time(0); + term->update_nav_data(a, s, time); } void operator()() From 7d3e20e3d9b8b0c44040b2f8739a125574e590a4 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 22:14:03 +0200 Subject: [PATCH 17/17] keyboard should work ands show /cmd_vel --- src/display.cpp | 36 +++++++++++++++++++++++++++++++++--- src/display.h | 7 +++++++ src/keyboard_cmd.cpp | 27 ++++++++++++++++++++++----- 3 files changed, 62 insertions(+), 8 deletions(-) diff --git a/src/display.cpp b/src/display.cpp index 333a258..fae4dc9 100644 --- a/src/display.cpp +++ b/src/display.cpp @@ -1,5 +1,6 @@ #include #include +#include #include "display.h" const int Curses::cmd_kbd_lines = 8; @@ -12,10 +13,13 @@ const int Curses::get_lines = 1; const int Curses::get_columns = 1; const int Curses::nav_data_lines = 3; -const int Curses::nav_data_columns = 55; +const int Curses::nav_data_columns = 25; const int Curses::log_sent_w_lines = 12; -const int Curses::log_sent_w_columns = 40; +const int Curses::log_sent_w_columns = 22; + +const int Curses::topic_lines = 8; +const int Curses::topic_columns = 22; Curses::Curses() { initscr(); @@ -49,9 +53,17 @@ Curses::Curses() { wattron(nav_data, COLOR_PAIR(2)); wattron(nav_data, A_BOLD); + topic = newwin(topic_lines, topic_columns, + cmd_kbd_lines + get_lines + cmd_speed_lines + 1, + nav_data_columns + 1); + init_pair(3, COLOR_BLUE, COLOR_BLACK); + wattron(topic, COLOR_PAIR(3)); + wattron(topic, A_BOLD); + print_nav_data(); print_cmd_kbd(); print_cmd_speed(); + print_topic(); wmove(get, 0, 0); wrefresh(get); @@ -64,6 +76,7 @@ Curses::~Curses() { delwin(log_sent_title); delwin(nav_data); delwin(get); + delwin(topic); endwin(); } @@ -134,7 +147,7 @@ void Curses::update_nav_data(const float& batteryPercent, const int& state, const float& time) { wmove(nav_data, 0, 10); - wprintw(nav_data, "%f %", batteryPercent); + wprintw(nav_data, "%f %%", batteryPercent); wmove(nav_data, 2, 10); wprintw(nav_data, "%f %", time); wmove(nav_data, 1, 10); @@ -174,3 +187,20 @@ void Curses::update_nav_data(const float& batteryPercent, } wrefresh(nav_data); } + +void Curses::print_topic() { + wmove(topic, 0, 0); + waddstr(topic, "Linear :\n x : \n y : \n z : \n"); + waddstr(topic, "Angular :\n x : \n y : \n z : "); + wrefresh(topic); +} + +void Curses::update_topic(const geometry_msgs::Twist::ConstPtr& twist) { + wmove(topic, 1, 5); wprintw(topic, "%f ", twist->linear.x); + wmove(topic, 2, 5); wprintw(topic, "%f ", twist->linear.y); + wmove(topic, 3, 5); wprintw(topic, "%f ", twist->linear.z); + wmove(topic, 5, 5); wprintw(topic, "%f ", twist->angular.x); + wmove(topic, 6, 5); wprintw(topic, "%f ", twist->angular.y); + wmove(topic, 7, 5); wprintw(topic, "%f ", twist->angular.z); + wrefresh(topic); +} diff --git a/src/display.h b/src/display.h index 5afc6e7..2c88eda 100644 --- a/src/display.h +++ b/src/display.h @@ -3,6 +3,7 @@ #include #include +#include class Curses { @@ -32,6 +33,11 @@ class Curses WINDOW* nav_data; void print_nav_data(); + static const int topic_lines; + static const int topic_columns; + WINDOW* topic; + void print_topic(); + public: Curses(); ~Curses(); @@ -43,6 +49,7 @@ class Curses const int& state, const float& time); void log_sent(const std::string& str); + void update_topic(const geometry_msgs::Twist::ConstPtr& twist); }; #endif diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index e219e5a..0353d15 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -8,19 +8,33 @@ #include #include -class Callback +class NavdataCallback { private: boost::shared_ptr term; public: - Callback(const boost::shared_ptr& terminal) : + NavdataCallback(const boost::shared_ptr& terminal) : term(terminal) {} void operator()(const ardrone_autonomy::Navdata::ConstPtr& msg) { term->update_nav_data(msg->batteryPercent, msg->state, msg->tm); } -}; // class Callback +}; // class NavdataCallback + +class CmdVelCallback +{ + private: + boost::shared_ptr term; + + public: + CmdVelCallback(const boost::shared_ptr& terminal) : + term(terminal) {} + + void operator()(const geometry_msgs::Twist::ConstPtr& msg) { + term->update_topic(msg); + } +}; // class CmdVelCallback class Run { @@ -29,17 +43,19 @@ class Run ros::NodeHandle n; ros::Rate loop_rate; ros::Publisher cmd, pub_takeoff, pub_land, pub_reset; - ros::Subscriber data_subscriber; + ros::Subscriber data_subscriber, cmdvel_subscriber; void land() { pub_land.publish(empty); }; void takeoff() { pub_takeoff.publish(empty); }; void reset() { pub_reset.publish(empty); }; float x_speed, y_speed, z_speed, turn; boost::shared_ptr term; - Callback data_callback; + NavdataCallback data_callback; + CmdVelCallback cmdvel_callback; public: Run(const boost::shared_ptr& terminal) : data_callback(terminal), + cmdvel_callback(terminal), term(terminal), loop_rate(30), x_speed(0.2), @@ -51,6 +67,7 @@ class Run pub_land = n.advertise("/ardrone/land", 1); pub_reset = n.advertise("/ardrone/reset", 1); data_subscriber = n.subscribe("/ardrone/navdata", 1, data_callback); + cmdvel_subscriber = n.subscribe("/cmd_vel", 1, cmdvel_callback); term->update_cmd_speed('x', x_speed); term->update_cmd_speed('y', y_speed);