Merge branch 'cppkeyboard'

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)

src/display.cpp

@@ -0,0 +1,206 @@
+#include <ncurses.h>
+#include <string>
+#include <geometry_msgs/Twist.h>
+#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);
+}

src/display.h

@@ -0,0 +1,55 @@
+#ifndef CURSES_DISPLAY
+#define CURSES_DISPLAY
+
+#include <ncurses.h>
+#include <string>
+#include <geometry_msgs/Twist.h>
+
+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

src/keyboard_cmd.cpp

@@ -1,15 +1,40 @@
-#include <termios.h>
-#include <iostream>
-#include <map>
-#include <vector>
-#include <string>
 #include <ros/ros.h>
 #include <ros/time.h>
-
+#include <locale.h>
 #include <ncurses.h>
+#include "display.h"
 
 #include <std_msgs/Empty.h>
 #include <geometry_msgs/Twist.h>
+#include <ardrone_autonomy/Navdata.h>
+
+class NavdataCallback
+{
+  private:
+    boost::shared_ptr<Curses> term;
+
+  public:
+    NavdataCallback(const boost::shared_ptr<Curses>& 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<Curses> term;
+
+  public:
+    CmdVelCallback(const boost::shared_ptr<Curses>& 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()
+    ros::Subscriber data_subscriber, cmdvel_subscriber;
-      { pub_land.publish(empty); };
+    void land() { pub_land.publish(empty); };
-    void takeoff()
+    void takeoff() { pub_takeoff.publish(empty); };
-      { pub_takeoff.publish(empty); };
+    void reset() { pub_reset.publish(empty); };
-    void reset()
-      { pub_reset.publish(empty); };
     float x_speed, y_speed, z_speed, turn;
-    void print_speed()
+    boost::shared_ptr<Curses> term;
-    {
+    NavdataCallback data_callback;
-      std::cout << "x speed : " << x_speed << ", y speed : " <<
+    CmdVelCallback cmdvel_callback;
-        y_speed << ", z speed : " << z_speed <<
-        ", rotation speed : " << turn << "\n";
-    };
 
   public:
-    Run() :
+    Run(const boost::shared_ptr<Curses>& terminal) :
-      n(),
+      data_callback(terminal),
-      empty(),
+      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<std_msgs::Empty>("/ardrone/takeoff", 1);
         pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 1);
         pub_reset = n.advertise<std_msgs::Empty>("/ardrone/reset", 1);
-      }
+        data_subscriber = n.subscribe<ardrone_autonomy::Navdata>("/ardrone/navdata", 1, data_callback);
+        cmdvel_subscriber = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdvel_callback);
 
-    ~Run()
+        term->update_cmd_speed('x', x_speed);
-    {
+        term->update_cmd_speed('y', y_speed);
-      endwin();
+        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
+              cmd.publish(msg);
-
+              term->log_sent("hover !");
-        ros::spinOnce();
+            }
+        } // switch(c)
         loop_rate.sleep();
-
       } // while
-    } // run
+    } //void run()
-}; // class
+
+}; // class Run
 
 int main(int argc, char** argv)
 {
+  setlocale(LC_ALL, "");
   ros::init(argc, argv, "keyboard_cmd");
-  Run run;
+  boost::shared_ptr<Curses> term(new Curses());
-  run();
+  Run fun(term);
+  fun();
   return 0;
-}
+} // main