add c++ ncurses drone commander

src/keyboard_cmd.cpp

@@ -0,0 +1,250 @@
+#include <termios.h>
+#include <iostream>
+#include <map>
+#include <vector>
+#include <string>
+#include <ros/ros.h>
+#include <ros/time.h>
+
+#include <ncurses.h>
+
+#include <std_msgs/Empty.h>
+#include <geometry_msgs/Twist.h>
+
+class Run
+{
+  private:
+    std_msgs::Empty empty;
+    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); };
+    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";
+    };
+
+  public:
+    Run() :
+      n(),
+      empty(),
+      loop_rate(30),
+      x_speed(0.2),
+      y_speed(0.3),
+      z_speed(0.5),
+      turn(0.5) {
+        cmd =  n.advertise<geometry_msgs::Twist>("/cmd_vel",1);
+        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);
+      }
+
+    ~Run()
+    {
+      endwin();
+    }
+
+    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())
+      {
+        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();
+
+        switch(c)
+        {
+          case 'k' :
+            {// hover
+              cmd.publish(msg);
+              break;
+            }
+          case 'i' :
+            {// forward
+              msg->linear.x = x_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case ';' :
+            {// backward
+              msg->linear.x = -x_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case 'h' :
+            {//translate left
+              msg->linear.y = -y_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case 'm' :
+            {//translate right
+              msg->linear.y = y_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case 'j' :
+            {//rotate left
+              msg->angular.z = turn;
+              cmd.publish(msg);
+              break;
+            }
+          case 'l' :
+            {//rotate right
+              msg->angular.z = -turn;
+              cmd.publish(msg);
+              break;
+            }
+          case 'u' :
+            {//turn left
+              msg->angular.z = turn;
+              msg->linear.x = x_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case 'o' :
+            {//turn right
+              msg->angular.z = -turn;
+              msg->linear.x = x_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case ',' :
+            {//turn left backward
+              msg->angular.z = turn;
+              msg->linear.x = -x_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case ':' :
+            {//turn right backward
+              msg->angular.z = -turn;
+              msg->linear.x = -x_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case 'y' :
+            {//up
+              msg->linear.z = z_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case 'n' :
+            {//down
+              msg->linear.z = -z_speed;
+              cmd.publish(msg);
+              break;
+            }
+          case 't' :
+            {//takeoff
+              takeoff();
+              break;
+            }
+          case 'b' :
+            {//land
+              land();
+              break;
+            }
+          case 'g' :
+            {//reset
+              reset();
+              break;
+            }
+          case 'a' :
+            {// + x_speed
+              x_speed *= 1.1;
+              print_speed();
+              break;
+            }
+          case 'w' :
+            {// - x_speed
+              x_speed *= 0.9;
+              print_speed();
+              break;
+            }
+          case 'z' :
+            {// + y_speed
+              y_speed *= 1.1;
+              print_speed();
+              break;
+            }
+          case 'x' :
+            {// - y_speed
+              y_speed *= 0.9;
+              print_speed();
+              break;
+            }
+          case 'e' :
+            {// + z_speed
+              z_speed *= 1.1;
+              print_speed();
+              break;
+            }
+          case 'c' :
+            {// - z_speed
+              z_speed *= 0.9;
+              print_speed();
+              break;
+            }
+          case 'r' :
+            {// + turn speed
+              turn *= 1.1;
+              print_speed();
+              break;
+            }
+          case 'v' :
+            {// - turn speed
+              turn *= 0.9;
+              print_speed();
+              break;
+            }
+          default :
+            break;
+        } // switch
+
+        ros::spinOnce();
+        loop_rate.sleep();
+
+      } // while
+    } // run
+}; // class
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "keyboard_cmd");
+  Run run;
+  run();
+  return 0;
+}
+