AllegroHand_keyboard.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 //#include <turtlesim/Velocity.h>
00003 #include "std_msgs/String.h"
00004 #include <signal.h>
00005 #include <termios.h>
00006 #include <stdio.h>
00007 
00008 #define KEYCODE_R 0x43 
00009 #define KEYCODE_L 0x44
00010 #define KEYCODE_U 0x41
00011 #define KEYCODE_D 0x42
00012 #define KEYCODE_Q 0x71
00013 
00014 class AllegroKeyboard
00015 {
00016 public:
00017   AllegroKeyboard();
00018   void keyLoop();
00019 
00020 private:
00021 
00022   
00023   ros::NodeHandle nh_;
00024   double linear_, angular_, l_scale_, a_scale_;
00025   ros::Publisher vel_pub_;
00026   
00027 };
00028 
00029 AllegroKeyboard::AllegroKeyboard():
00030   linear_(0),
00031   angular_(0),
00032   l_scale_(2.0),
00033   a_scale_(2.0)
00034 {
00035   nh_.param("scale_angular", a_scale_, a_scale_);
00036   nh_.param("scale_linear", l_scale_, l_scale_);
00037 
00038   vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
00039 }
00040 
00041 int kfd = 0;
00042 struct termios cooked, raw;
00043 
00044 void quit(int sig)
00045 {
00046   tcsetattr(kfd, TCSANOW, &cooked);
00047   ros::shutdown();
00048   exit(0);
00049 }
00050 
00051 
00052 int main(int argc, char** argv)
00053 {
00054   ros::init(argc, argv, "teleop_turtle");
00055   AllegroKeyboard teleop_turtle;
00056 
00057   signal(SIGINT,quit);
00058 
00059   teleop_turtle.keyLoop();
00060   
00061   return(0);
00062 }
00063 
00064 
00065 void AllegroKeyboard::keyLoop()
00066 {
00067   char c;
00068   bool dirty=false;
00069 
00070 
00071   // get the console in raw mode                                                              
00072   tcgetattr(kfd, &cooked);
00073   memcpy(&raw, &cooked, sizeof(struct termios));
00074   raw.c_lflag &=~ (ICANON | ECHO);
00075   // Setting a new line, then end of file                         
00076   raw.c_cc[VEOL] = 1;
00077   raw.c_cc[VEOF] = 2;
00078   tcsetattr(kfd, TCSANOW, &raw);
00079 
00080   puts("Reading from keyboard");
00081   puts("---------------------------");
00082   puts("Use arrow keys to move the turtle.");
00083 
00084 
00085   for(;;)
00086   {
00087     // get the next event from the keyboard  
00088     if(read(kfd, &c, 1) < 0)
00089     {
00090       perror("read():");
00091       exit(-1);
00092     }
00093 
00094     linear_=angular_=0;
00095     ROS_DEBUG("value: 0x%02X\n", c);
00096   
00097     switch(c)
00098     {
00099       case KEYCODE_L:
00100         ROS_DEBUG("LEFT");
00101         angular_ = 1.0;
00102         dirty = true;
00103         break;
00104       case KEYCODE_R:
00105         ROS_DEBUG("RIGHT");
00106         angular_ = -1.0;
00107         dirty = true;
00108         break;
00109       case KEYCODE_U:
00110         ROS_DEBUG("UP");
00111         linear_ = 1.0;
00112         dirty = true;
00113         break;
00114       case KEYCODE_D:
00115         ROS_DEBUG("DOWN");
00116         linear_ = -1.0;
00117         dirty = true;
00118         break;
00119     }
00120    
00121 
00122     turtlesim::Velocity vel;
00123     vel.angular = a_scale_*angular_;
00124     vel.linear = l_scale_*linear_;
00125     if(dirty ==true)
00126     {
00127       vel_pub_.publish(vel);    
00128       dirty=false;
00129     }
00130   }
00131 
00132 
00133   return;
00134 }
00135 
00136 
00137 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


AllegroHand_keyboard
Author(s): Alex
autogenerated on Thu Mar 21 2013 21:24:03