teleop_turtle_key.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <geometry_msgs/Twist.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 twist_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   twist_pub_ = nh_.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
00038 }
00039 
00040 int kfd = 0;
00041 struct termios cooked, raw;
00042 
00043 void quit(int sig)
00044 {
00045   (void)sig;
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   TeleopTurtle teleop_turtle;
00056 
00057   signal(SIGINT,quit);
00058 
00059   teleop_turtle.keyLoop();
00060   
00061   return(0);
00062 }
00063 
00064 
00065 void TeleopTurtle::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     geometry_msgs::Twist twist;
00123     twist.angular.z = a_scale_*angular_;
00124     twist.linear.x = l_scale_*linear_;
00125     if(dirty ==true)
00126     {
00127       twist_pub_.publish(twist);    
00128       dirty=false;
00129     }
00130   }
00131 
00132 
00133   return;
00134 }
00135 
00136 
00137 


turtlesim
Author(s): Josh Faust
autogenerated on Thu Jun 6 2019 20:20:12