diff --git a/CMakeLists.txt b/CMakeLists.txt index 4bc809a..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 @@ -21,10 +27,13 @@ generate_messages( geometry_msgs ) -catkin_package() +catkin_package( +CATKIN_DEPENDS message_runtime +) include_directories( ${catkin_INCLUDE_DIRS} + ${ncursesw_INCLUDE_DIRS} ) add_executable(filtre src/filtre.cpp) @@ -36,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/README.md b/README.md index 0bf59a2..7fd3f57 100644 --- a/README.md +++ b/README.md @@ -1,29 +1,20 @@ -# 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-freenect-stack ros-indigo-ardrone-autonomy libncursesw5-dev +``` ## Utilisation du dépôt ## Après avoir créé un espace de travail catkin : - ``` #!sh source /opt/ros/indigo/setup.bash @@ -45,7 +36,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 : @@ -65,4 +55,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. 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/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..96bf5cb 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -1,15 +1,39 @@ -#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 +42,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 +65,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 +146,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 +154,7 @@ class Run msg->angular.z = -turn; msg->linear.x = x_speed; cmd.publish(msg); + term->log_sent("forward right !"); break; } case ',' : @@ -145,6 +162,7 @@ class Run msg->angular.z = turn; msg->linear.x = -x_speed; cmd.publish(msg); + term->log_sent("backward left !"); break; } case ':' : @@ -152,99 +170,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 diff --git a/src/normal_estimator.cpp b/src/normal_estimator.cpp index 26dd806..8a30d83 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,10 +29,11 @@ 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.seq, msg->header.stamp)); } - Callback(ros::Publisher& pub) : publisher(pub), estimator() {} + Callback(ros::Publisher& pub) : + publisher(pub), estimator() {} private: ros::Publisher publisher; @@ -41,7 +43,7 @@ class Callback { 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, uint32_t seq, 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 = 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