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


turtlesim
Author(s): Josh Faust
autogenerated on Mon Oct 6 2014 06:54:52