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/26] 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/26] =?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/26] 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/26] 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/26] 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/26] 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/26] 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/26] 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 0901936351e5ad39cf563308bdd5721203ad6e0b Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 18:02:58 +0200 Subject: [PATCH 09/26] 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 4bc809a..d5862bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,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 0185fc832e94e1e4f7961a9726d5dec6e3c4ebf7 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 18:04:56 +0200 Subject: [PATCH 10/26] 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 418b0cef95bf7dec44e8f9b369da1a91a589d401 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 18:28:40 +0200 Subject: [PATCH 11/26] 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 e70baf354f5fb25cd4b81c70a3dacf2db043472a Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sat, 2 May 2015 18:02:58 +0200 Subject: [PATCH 12/26] 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 13/26] 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 14/26] 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 15/26] 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 16/26] 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 17/26] 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 18/26] 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 19/26] 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 20/26] 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); From 04ec1dc21e40e23940bee409c9d4bc811ec4b238 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume Dubois Date: Sun, 3 May 2015 09:26:50 +0000 Subject: [PATCH 21/26] README.md edited online with Bitbucket --- README.md | 25 +++++++++---------------- 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index 0bf59a2..49e7e68 100644 --- a/README.md +++ b/README.md @@ -1,25 +1,18 @@ -# IMPORTANT # -Changement d’url du dépôt. Exécutez la commande ci-dessous à la racine de votre dépôt local : - -``` -#!sh - -git remote set-url origin git@bitbucket.org:_Luc_/hand_control.git -``` -ou : - -``` -#!sh - -git remote set-url origin https://username@bitbucket.org/_Luc_/hand_control.git # changer "username" -``` - # Contrôle par geste d'un drone # **Extrait de la présentation du projet** « On s'intéresse dans ce projet à contrôler un drone à l'aide la main. On utilisera pour ce faire une kinect, placée à l'horizontal, au dessus de laquelle on placera la main du contrôleur. La kinect fournit des informations sur la profondeur des objets placés en face d'elle. On peut alors régresser un plan sur les échantillons et utiliser deux inclinaisons et la distance pour contrôler le roulis, le tangage et l'altitude d'un drone. » +## Installation des dépendances ## + +``` +#!sh +sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' +wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add - +sudo apt-get update +sudo apt-get install ros-indigo-desktop-full ros-indigo-libfreenect ros-indigo-freenect-stack ros-indigo-ardrone-autonomy libncursesw5 libncursesw5-dev +``` ## Utilisation du dépôt ## Après avoir créé un espace de travail catkin : From d17011e16f71d965d90f31aefd918daa92a6676c Mon Sep 17 00:00:00 2001 From: Louis-Guillaume Dubois Date: Sun, 3 May 2015 09:29:58 +0000 Subject: [PATCH 22/26] README.md edited online with Bitbucket --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 49e7e68..0ad06dd 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add - sudo apt-get update -sudo apt-get install ros-indigo-desktop-full ros-indigo-libfreenect ros-indigo-freenect-stack ros-indigo-ardrone-autonomy libncursesw5 libncursesw5-dev +sudo apt-get install ros-indigo-desktop-full ros-indigo-freenect-stack ros-indigo-ardrone-autonomy libncursesw5-dev ``` ## Utilisation du dépôt ## From 64e35658b8972322d3a6d28d7fec9dde6dd5ec71 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume Dubois Date: Sun, 3 May 2015 09:31:27 +0000 Subject: [PATCH 23/26] README.md edited online with Bitbucket --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 0ad06dd..f59cdf1 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,6 @@ Le contenu du dépôt se trouve alors dans «~/hand_control_ws/src». Il est ens #!sh cd ~/hand_control_ws catkin_make -catkin_make # régle le problème du message "Plan" ``` Puis pour faciliter le développement : From a20988d443149d5be3c32adb6d5e1cac0df3c2da Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Sun, 3 May 2015 12:14:27 +0200 Subject: [PATCH 24/26] rm useless #include --- src/keyboard_cmd.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index 0353d15..96bf5cb 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -1,7 +1,6 @@ #include #include #include -#include #include "display.h" #include From 13112526dd5cbab54b50248fc06c15b1956f21af Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Tue, 5 May 2015 18:14:14 +0200 Subject: [PATCH 25/26] test jenkins --- README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/README.md b/README.md index f59cdf1..02d4ba7 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,6 @@ « On s'intéresse dans ce projet à contrôler un drone à l'aide la main. On utilisera pour ce faire une kinect, placée à l'horizontal, au dessus de laquelle on placera la main du contrôleur. La kinect fournit des informations sur la profondeur des objets placés en face d'elle. On peut alors régresser un plan sur les échantillons et utiliser deux inclinaisons et la distance pour contrôler le roulis, le tangage et l'altitude d'un drone. » ## Installation des dépendances ## - ``` #!sh sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' @@ -57,4 +56,4 @@ Avant de coder, regarder : - [les conventions d’écriture du code du projet ROS](http://wiki.ros.org/CppStyleGuide) - [le guide du développeur](http://wiki.ros.org/DevelopersGuide) -Cf. le [Wiki](https://bitbucket.org/_Luc_/handcontrol/wiki/Home) pour le reste de la documentation et le résultat des recherches. \ No newline at end of file +Cf. le [Wiki](https://bitbucket.org/_Luc_/handcontrol/wiki/Home) pour le reste de la documentation et le résultat des recherches. From fe9b8ef141ddf87c2841eb34b4e879f240fed3af Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Tue, 5 May 2015 18:16:35 +0200 Subject: [PATCH 26/26] test jenkins 2 --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 02d4ba7..7fd3f57 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,6 @@ sudo apt-get install ros-indigo-desktop-full ros-indigo-freenect-stack ros-indig ## Utilisation du dépôt ## Après avoir créé un espace de travail catkin : - ``` #!sh source /opt/ros/indigo/setup.bash