From 6eac98cab3f8f3b773761c66cfa98fc55688b4da Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Sat, 2 May 2015 00:40:28 +0200
Subject: [PATCH] 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 <ncurses.h>
 
@@ -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 <termios.h>
-#include <iostream>
-#include <map>
-#include <vector>
-#include <string>
 #include <ros/ros.h>
 #include <ros/time.h>
 #include <locale.h>
@@ -27,7 +22,6 @@ class Run
       { pub_reset.publish(empty); };
     float x_speed, y_speed, z_speed, turn;
     boost::shared_ptr<Curses> term;
-    void print_speed() { ; };
 
   public:
     Run(boost::shared_ptr<Curses> 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<Curses> term(new Curses());
   Run fun(term);
   fun();
   return 0;
-}
+} // main