SHARE
TWEET

playerros_teleop.cpp

bricerebsamen May 30th, 2011 247 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /* Control the robot from the keyboard.
  2. Use arrow keys to steer the robot. Space to stop it.
  3. Parameters scale_linear and scale_angular control the velocities sent to the robot.
  4. */
  5.  
  6. #include <ros/ros.h>
  7. #include <geometry_msgs/Twist.h>
  8.  
  9. #include <signal.h>
  10. #include <termios.h>
  11. #include <stdio.h>
  12.  
  13. #define KEYCODE_U 0x41
  14. #define KEYCODE_D 0x42
  15. #define KEYCODE_R 0x43
  16. #define KEYCODE_L 0x44
  17. #define KEYCODE_SPACE 0x20
  18.  
  19.  
  20.  
  21. class Teleop {
  22. public:
  23.   Teleop();
  24.   void keyLoop();
  25.  
  26. private:
  27.   ros::NodeHandle nh_;
  28.   double linear_, angular_, l_scale_, a_scale_;
  29.   ros::Publisher vel_pub_;
  30. };
  31.  
  32.  
  33. Teleop::Teleop():
  34.   linear_(0),
  35.   angular_(0),
  36.   l_scale_(1.0),
  37.   a_scale_(1.0)
  38. {
  39.   nh_.param("scale_angular", a_scale_, a_scale_);
  40.   nh_.param("scale_linear", l_scale_, l_scale_);
  41.  
  42.   vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
  43. }
  44.  
  45. int kfd = 0;
  46. struct termios cooked, raw;
  47.  
  48. void quit(int sig)
  49. {
  50.   tcsetattr(kfd, TCSANOW, &cooked);
  51.   ros::shutdown();
  52.   exit(0);
  53. }
  54.  
  55.  
  56. void Teleop::keyLoop()
  57. {
  58.   char c;
  59.   bool dirty=false;
  60.  
  61.  
  62.   // get the console in raw mode
  63.   tcgetattr(kfd, &cooked);
  64.   memcpy(&raw, &cooked, sizeof(struct termios));
  65.   raw.c_lflag &=~ (ICANON | ECHO);
  66.   // Setting a new line, then end of file
  67.   raw.c_cc[VEOL] = 1;
  68.   raw.c_cc[VEOF] = 2;
  69.   tcsetattr(kfd, TCSANOW, &raw);
  70.  
  71.   puts("Reading from keyboard");
  72.   puts("---------------------------");
  73.   puts("Use arrow keys to move the robot.");
  74.  
  75.  
  76.   for(;;)
  77.   {
  78.     // get the next event from the keyboard
  79.     if(read(kfd, &c, 1) < 0)
  80.     {
  81.       perror("read():");
  82.       exit(-1);
  83.     }
  84.  
  85.     linear_=angular_=0;
  86.     ROS_DEBUG("value: 0x%02X\n", c);
  87.  
  88.     switch(c)
  89.     {
  90.       case KEYCODE_L:
  91.         ROS_DEBUG("LEFT");
  92.         angular_ = 1.0;
  93.         dirty = true;
  94.         break;
  95.       case KEYCODE_R:
  96.         ROS_DEBUG("RIGHT");
  97.         angular_ = -1.0;
  98.         dirty = true;
  99.         break;
  100.       case KEYCODE_U:
  101.         ROS_DEBUG("UP");
  102.         linear_ = 1.0;
  103.         dirty = true;
  104.         break;
  105.       case KEYCODE_D:
  106.         ROS_DEBUG("DOWN");
  107.         linear_ = -1.0;
  108.         dirty = true;
  109.         break;
  110.       case KEYCODE_SPACE:
  111.         ROS_DEBUG("SPACE/STOP");
  112.         linear_ = 0.0;
  113.         angular_ = 0.0;
  114.         dirty = true;
  115.         break;
  116.     }
  117.  
  118.  
  119.     geometry_msgs::Twist vel;
  120.     vel.angular.z = a_scale_*angular_;
  121.     vel.linear.x = l_scale_*linear_;
  122.     if( dirty )
  123.     {
  124.       vel_pub_.publish(vel);
  125.       dirty = false;
  126.     }
  127.   }
  128.  
  129.   return;
  130. }
  131.  
  132.  
  133. int main(int argc, char** argv)
  134. {
  135.   ros::init(argc, argv, "teleop");
  136.   Teleop teleop;
  137.  
  138.   signal(SIGINT,quit);
  139.  
  140.   teleop.keyLoop();
  141.  
  142.   return(0);
  143. }
RAW Paste Data
Top