Robot Operating System (ROS)学习笔记3---键盘控制

搭建环境：XMWare  Ubuntu14.04  ROS（indigo）

转载自古月居  转载连接：http://www.guyuehome.com/253

一、创建控制包

 catkin_create-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
 catkin_make

建包，参考：http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage

二、简单的消息发布

　　在smartcar_teleop下 创建scripts文件夹，在其文件夹下创建teleop.py文件

 #!/usr/bin/env python
 import roslib;
 roslib.load_manifest('smartcar_teleop')
 import rospy
 from geometry_msgs.msg import Twist
 from std_msgs.msg import String

 class Teleop:
     """docstring fos Teleop"""
     def __init__(self):
         pub = rospy.Publisher('cmd_vel',Twist)
         rospy.init_node('smartcar_teleop')
         rate = rospy.Rate(rospy.get_param('~hz',1))
         self.cmd = None

         cmd =Twist()
         cmd.linear.x = 0.2
         cmd.linear.y = 0
         cmd.linear.z = 0

         cmd.angular.x = 0
         cmd.angular.y = 0
         cmd.angular.z = 0.5

         self.cmd = cmd

         while not rospy.is_shutdown():
             str = "Hello world %s" %rospy.get_time()
             rospy.loginfo(str)
             pub.publish(self.cmd)
             rate.sleep()
 if __name__ == "__main__":Teleop()

先运行之前教程中用到的smartcar机器人，在rviz中进行显示

然后新建终端，输入如下命令：     

 sudo chmod +x teleop.py
 rosrun smartcar_teleop teleop.py 

可以建立一个launch文件（teleop.launch）运行

 <launch>
   <arg name="cmd_topic" default="cmd_vel" />
   <node pkg="smartcar_teleop" type="teleop.py" name="smartcar_teleop">
     <remap from="cmd_vel" to="$(arg cmd_topic)" />
   </node>
 </launch>

三、加入键盘控制

1、移植

　　首先，我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下，然后修改CMakeLists.txt文件，将下列代码加入文件底部：

 add_executable(smartcar_keyboard_teleop src/keyboard.cpp)
 target_link_libraries(smartcar_keyboard_teleop boost_thread ${catkin_LIBRARIES})

keyboard.cpp代码如下：

 #include <termios.h>
 #include <signal.h>
 #include <math.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <sys/poll.h>  

 #include <boost/thread/thread.hpp>
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>  

 #define KEYCODE_W 0x77
 #define KEYCODE_A 0x61
 #define KEYCODE_S 0x73
 #define KEYCODE_D 0x64  

 #define KEYCODE_A_CAP 0x41
 #define KEYCODE_D_CAP 0x44
 #define KEYCODE_S_CAP 0x53
 #define KEYCODE_W_CAP 0x57  

 class SmartCarKeyboardTeleopNode
 {
     private:
         double walk_vel_;
         double run_vel_;
         double yaw_rate_;
         double yaw_rate_run_;  

         geometry_msgs::Twist cmdvel_;
         ros::NodeHandle n_;
         ros::Publisher pub_;  

     public:
         SmartCarKeyboardTeleopNode()
         {
             pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);  

             ros::NodeHandle n_private("~");
             n_private.param("walk_vel", walk_vel_, 0.5);
             n_private.param("run_vel", run_vel_, 1.0);
             n_private.param("yaw_rate", yaw_rate_, 1.0);
             n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
         }  

         ~SmartCarKeyboardTeleopNode() { }
         void keyboardLoop();  

         void stopRobot()
         {
             cmdvel_.linear.x = 0.0;
             cmdvel_.angular.z = 0.0;
             pub_.publish(cmdvel_);
         }
 };  

 SmartCarKeyboardTeleopNode* tbk;
 int kfd = 0;
 struct termios cooked, raw;
 bool done;  

 int main(int argc, char** argv)
 {
     ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
     SmartCarKeyboardTeleopNode tbk;  

     boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));  

     ros::spin();  

     t.interrupt();
     t.join();
     tbk.stopRobot();
     tcsetattr(kfd, TCSANOW, &cooked);  

     return(0);
 }  

 void SmartCarKeyboardTeleopNode::keyboardLoop()
 {
     char c;
     double max_tv = walk_vel_;
     double max_rv = yaw_rate_;
     bool dirty = false;
     int speed = 0;
     int turn = 0;  

     // get the console in raw mode
     tcgetattr(kfd, &cooked);
     memcpy(&raw, &cooked, sizeof(struct termios));
     raw.c_lflag &=~ (ICANON | ECHO);
     raw.c_cc[VEOL] = 1;
     raw.c_cc[VEOF] = 2;
     tcsetattr(kfd, TCSANOW, &raw);  

     puts("Reading from keyboard");
     puts("Use WASD keys to control the robot");
     puts("Press Shift to move faster");  

     struct pollfd ufd;
     ufd.fd = kfd;
     ufd.events = POLLIN;  

     for(;;)
     {
         boost::this_thread::interruption_point();  

         // get the next event from the keyboard
         int num;  

         if ((num = poll(&ufd, 1, 250)) <)
         {
             perror("poll():");
             return;
         }
         else if(num > 0)
         {
             if(read(kfd, &c, 1) <)
             {
                 perror("read():");
                 return;
             }
         }
         else
         {
             if (dirty == true)
             {
                 stopRobot();
                 dirty = false;
             }  

             continue;
         }  

         switch(c)
         {
             case KEYCODE_W:
                 max_tv = walk_vel_;
                 speed = 1;
                 turn = 0;
                 dirty = true;
                 break;
             case KEYCODE_S:
                 max_tv = walk_vel_;
                 speed = -1;
                 turn = 0;
                 dirty = true;
                 break;
             case KEYCODE_A:
                 max_rv = yaw_rate_;
                 speed = 0;
                 turn = 1;
                 dirty = true;
                 break;
             case KEYCODE_D:
                 max_rv = yaw_rate_;
                 speed = 0;
                 turn = -1;
                 dirty = true;
                 break;  

             case KEYCODE_W_CAP:
                 max_tv = run_vel_;
                 speed = 1;
                 turn = 0;
                 dirty = true;
                 break;
             case KEYCODE_S_CAP:
                 max_tv = run_vel_;
                 speed = -1;
                 turn = 0;
                 dirty = true;
                 break;
             case KEYCODE_A_CAP:
                 max_rv = yaw_rate_run_;
                 speed = 0;
                 turn = 1;
                 dirty = true;
                 break;
             case KEYCODE_D_CAP:
                 max_rv = yaw_rate_run_;
                 speed = 0;
                 turn = -1;
                 dirty = true;
                 break;
             default:
                 max_tv = walk_vel_;
                 max_rv = yaw_rate_;
                 speed = 0;
                 turn = 0;
                 dirty = false;
         }
         cmdvel_.linear.x = speed * max_tv;
         cmdvel_.angular.z = turn * max_rv;
         pub_.publish(cmdvel_);
     }
 }

编译完成后，运行smartcar模型。重新打开一个终端，打开键盘控制节点：

rosrun smartcar_teleop smartcar_keyboard_teleop

在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。