diff --git a/CMakeLists.txt b/CMakeLists.txt index d5862bb..c8c9ac3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,13 @@ find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs message_generation + ardrone_autonomy ) +find_package( + PkgConfig REQUIRED +) + +pkg_check_modules ( ncursesw REQUIRED ncursesw) add_message_files( FILES @@ -27,6 +33,7 @@ CATKIN_DEPENDS message_runtime include_directories( ${catkin_INCLUDE_DIRS} + ${ncursesw_INCLUDE_DIRS} ) add_executable(filtre src/filtre.cpp) @@ -38,6 +45,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(display src/display.cpp) + +add_executable(keyboard_cmd src/keyboard_cmd.cpp) +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}) add_dependencies(normal_estimator hand_control_generate_messages_cpp) diff --git a/src/display.cpp b/src/display.cpp new file mode 100644 index 0000000..fae4dc9 --- /dev/null +++ b/src/display.cpp @@ -0,0 +1,206 @@ +#include +#include +#include +#include "display.h" + +const int Curses::cmd_kbd_lines = 8; +const int Curses::cmd_kbd_columns = 55; + +const int Curses::cmd_speed_lines = 4; +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 = 3; +const int Curses::nav_data_columns = 25; + +const int Curses::log_sent_w_lines = 12; +const int Curses::log_sent_w_columns = 22; + +const int Curses::topic_lines = 8; +const int Curses::topic_columns = 22; + +Curses::Curses() { + initscr(); + cbreak(); + start_color(); + //noecho(); + + cmd_kbd = newwin(cmd_kbd_lines, cmd_kbd_columns, 0, 0); + + 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_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); + 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); + + 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); +} + +Curses::~Curses() { + delwin(cmd_kbd); + delwin(cmd_speed); + delwin(log_sent_w); + delwin(log_sent_title); + delwin(nav_data); + delwin(get); + delwin(topic); + endwin(); +} + +char Curses::getchar() { + return wgetch(get); +} + +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, " Press ctrl + C to quit"); + 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) { + wmove(log_sent_w, log_line_number++, 0); + waddstr(log_sent_w, (str + "\n").c_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_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); +} + +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 new file mode 100644 index 0000000..2c88eda --- /dev/null +++ b/src/display.h @@ -0,0 +1,55 @@ +#ifndef CURSES_DISPLAY +#define CURSES_DISPLAY + +#include +#include +#include + +class Curses +{ + private: + 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_speed(); + + static const int get_lines; + static const int get_columns; + WINDOW* get; + + static const int log_sent_w_lines; + 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; + 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(); + char getchar(); + + // TODO + void update_cmd_speed(const char& coord, const float& v); + void update_nav_data(const float& batteryPercent, + 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 ac52ac1..0353d15 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -1,15 +1,40 @@ -#include -#include -#include -#include -#include #include #include - +#include #include +#include "display.h" #include #include +#include + +class NavdataCallback +{ + private: + boost::shared_ptr term; + + public: + 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 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 { @@ -18,24 +43,20 @@ 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, 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; - void print_speed() - { - std::cout << "x speed : " << x_speed << ", y speed : " << - y_speed << ", z speed : " << z_speed << - ", rotation speed : " << turn << "\n"; - }; + boost::shared_ptr term; + NavdataCallback data_callback; + CmdVelCallback cmdvel_callback; public: - Run() : - n(), - empty(), + Run(const boost::shared_ptr& terminal) : + data_callback(terminal), + cmdvel_callback(terminal), + term(terminal), loop_rate(30), x_speed(0.2), y_speed(0.3), @@ -45,85 +66,80 @@ 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); + cmdvel_subscriber = n.subscribe("/cmd_vel", 1, cmdvel_callback); - ~Run() - { - endwin(); - } + 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); + + float a(0); + int s(0); + float time(0); + term->update_nav_data(a, s, time); + } 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"; - */ - - initscr(); - cbreak(); - noecho(); - 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.; - char c = getch(); + char c = term->getchar(); switch(c) { 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' : @@ -131,6 +147,7 @@ class Run msg->angular.z = turn; msg->linear.x = x_speed; cmd.publish(msg); + term->log_sent("forward left !"); break; } case 'o' : @@ -138,6 +155,7 @@ class Run msg->angular.z = -turn; msg->linear.x = x_speed; cmd.publish(msg); + term->log_sent("forward right !"); break; } case ',' : @@ -145,6 +163,7 @@ class Run msg->angular.z = turn; msg->linear.x = -x_speed; cmd.publish(msg); + term->log_sent("backward left !"); break; } case ':' : @@ -152,99 +171,108 @@ 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_cmd_speed('x', x_speed); break; } case 'w' : {// - x_speed x_speed *= 0.9; - print_speed(); + term->update_cmd_speed('x', x_speed); break; } case 'z' : {// + y_speed y_speed *= 1.1; - print_speed(); + term->update_cmd_speed('y', y_speed); break; } case 'x' : {// - y_speed y_speed *= 0.9; - print_speed(); + term->update_cmd_speed('y', y_speed); break; } case 'e' : {// + z_speed z_speed *= 1.1; - print_speed(); + term->update_cmd_speed('z', z_speed); break; } case 'c' : {// - z_speed z_speed *= 0.9; - print_speed(); + term->update_cmd_speed('z', z_speed); break; } case 'r' : {// + turn speed turn *= 1.1; - print_speed(); + term->update_cmd_speed('t', turn); break; } case 'v' : {// - turn speed turn *= 0.9; - print_speed(); + term->update_cmd_speed('t', turn); break; } default : - break; - } // switch - - ros::spinOnce(); + { + cmd.publish(msg); + term->log_sent("hover !"); + } + } // switch(c) loop_rate.sleep(); - } // while - } // run -}; // class + } //void run() + +}; // class Run int main(int argc, char** argv) { + setlocale(LC_ALL, ""); ros::init(argc, argv, "keyboard_cmd"); - Run run; - run(); + boost::shared_ptr term(new Curses()); + Run fun(term); + fun(); return 0; -} +} // main