From 7d3e20e3d9b8b0c44040b2f8739a125574e590a4 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Sat, 2 May 2015 22:14:03 +0200
Subject: [PATCH] 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 <ncurses.h>
 #include <string>
+#include <geometry_msgs/Twist.h>
 #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 <ncurses.h>
 #include <string>
+#include <geometry_msgs/Twist.h>
 
 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 <geometry_msgs/Twist.h>
 #include <ardrone_autonomy/Navdata.h>
 
-class Callback
+class NavdataCallback
 {
   private:
     boost::shared_ptr<Curses> term;
 
   public:
-    Callback(const boost::shared_ptr<Curses>& terminal) :
+    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 Callback
+}; // 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
 {
@@ -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<Curses> term;
-    Callback data_callback;
+    NavdataCallback data_callback;
+    CmdVelCallback cmdvel_callback;
 
   public:
     Run(const boost::shared_ptr<Curses>& 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<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);
 
         term->update_cmd_speed('x', x_speed);
         term->update_cmd_speed('y', y_speed);