如果尝试过前面的例子，每次让机器人移动还要在终端里输入指令太麻烦了，有没有办法通过键盘来控制机器人的移动呢？答案室当然的了。在研究了机器人键盘控制的代码，最后移植到了smartcar上，实验成功。

一、创建控制包

        首先，我们为键盘控制单独建立一个包：

1. [plain] view plaincopy
   
2. roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp  
   
3. rosmake  

复制代码

二、简单的消息发布

        在机器人仿真中，主要控制机器人移动的就是 在机器人仿真中，主要控制机器人移动的就是Twist()结构，如果我们编程将这个结构通过程序发布成topic，自然就可以控制机器人了。我们先用简单的python来尝试一下。

        之前的模拟中，我们使用的都是在命令行下进行的消息发布，现在我们需要把这些命令转换成python代码，封装到一个单独的节点中去。针对之前的命令行，我们可以很简单的在smartcar_teleop /scripts文件夹下编写如下的控制代码：

1. [python] view plaincopy
   
2. #!/usr/bin/env python  
   
3. import roslib; roslib.load_manifest('smartcar_teleop')  
   
4. import rospy  
   
5. from geometry_msgs.msg import Twist  
   
6. from std_msgs.msg import String  
   
7.   
   
8. class Teleop:  
   
9.     def __init__(self):  
   
10.         pub = rospy.Publisher('cmd_vel', Twist)  
    
11.         rospy.init_node('smartcar_teleop')  
    
12.         rate = rospy.Rate(rospy.get_param('~hz', 1))  
    
13.         self.cmd = None  
    
14.       
    
15.         cmd = Twist()  
    
16.         cmd.linear.x = 0.2  
    
17.         cmd.linear.y = 0  
    
18.         cmd.linear.z = 0  
    
19.         cmd.angular.z = 0  
    
20.         cmd.angular.z = 0  
    
21.         cmd.angular.z = 0.5  
    
22.   
    
23.         self.cmd = cmd  
    
24.         while not rospy.is_shutdown():  
    
25.             str = "hello world %s" % rospy.get_time()  
    
26.             rospy.loginfo(str)  
    
27.             pub.publish(self.cmd)  
    
28.             rate.sleep()  
    
29.   
    
30. if __name__ == '__main__':Teleop()  

复制代码

     python代码在ROS重视不需要编译的。先运行之前教程中用到的smartcar机器人，在rviz中进行显示，然后新建终端，输入如下命令：

1. [plain] view plaincopy
   
2. rosrun smartcar_teleop teleop.py  

复制代码

也可以建立一个launch文件运行：

1. [plain] view plaincopy
   
2. <launch>  
   
3.   <arg name="cmd_topic" default="cmd_vel" />  
   
4.   <node pkg="smartcar_teleop" type="teleop.py" name="smartcar_teleop">  
   
5.     <remap from="cmd_vel" to="$(arg cmd_topic)" />  
   
6.   </node>  
   
7. </launch>  

复制代码

在rviz中看一下机器人是不是动起来了！

三、加入键盘控制

        当然前边的程序不是我们要的，我们需要的键盘控制。

1、移植

        因为ROS的代码具有很强的可移植性，所以用键盘控制的代码其实可以直接从其他机器人包中移植过来，在这里我主要参考的是erratic_robot，在这个机器人的代码中有一个erratic_teleop包，可以直接移植过来使用。
        首先，我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下，然后修改CMakeLists.txt文件，将下列代码加入文件底部：

1. [plain] view plaincopy
   
2. rosbuild_add_boost_directories()  
   
3. rosbuild_add_executable(smartcar_keyboard_teleop src/keyboard.cpp)  
   
4. target_link_libraries(smartcar_keyboard_teleop boost_thread)  

复制代码

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

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

2、复用

       因为代码是我们直接复制过来的，其中有很多与之前erratic机器人相关的变量，我们把代码稍作修改，变成自己机器人可读性较强的代码。

1. [cpp] view plaincopy
   
2. #include <termios.h>  
   
3. #include <signal.h>  
   
4. #include <math.h>  
   
5. #include <stdio.h>  
   
6. #include <stdlib.h>  
   
7. #include <sys/poll.h>  
   
8.   
   
9. #include <boost/thread/thread.hpp>  
   
10. #include <ros/ros.h>  
    
11. #include <geometry_msgs/Twist.h>  
    
12.   
    
13. #define KEYCODE_W 0x77  
    
14. #define KEYCODE_A 0x61  
    
15. #define KEYCODE_S 0x73  
    
16. #define KEYCODE_D 0x64  
    
17.   
    
18. #define KEYCODE_A_CAP 0x41  
    
19. #define KEYCODE_D_CAP 0x44  
    
20. #define KEYCODE_S_CAP 0x53  
    
21. #define KEYCODE_W_CAP 0x57  
    
22.   
    
23. class SmartCarKeyboardTeleopNode  
    
24. {  
    
25.     private:  
    
26.         double walk_vel_;  
    
27.         double run_vel_;  
    
28.         double yaw_rate_;  
    
29.         double yaw_rate_run_;  
    
30.          
    
31.         geometry_msgs::Twist cmdvel_;  
    
32.         ros::NodeHandle n_;  
    
33.         ros::Publisher pub_;  
    
34.   
    
35.     public:  
    
36.         SmartCarKeyboardTeleopNode()  
    
37.         {  
    
38.             pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);  
    
39.               
    
40.             ros::NodeHandle n_private("~");  
    
41.             n_private.param("walk_vel", walk_vel_, 0.5);  
    
42.             n_private.param("run_vel", run_vel_, 1.0);  
    
43.             n_private.param("yaw_rate", yaw_rate_, 1.0);  
    
44.             n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);  
    
45.         }  
    
46.          
    
47.         ~SmartCarKeyboardTeleopNode() { }  
    
48.         void keyboardLoop();  
    
49.          
    
50.         void stopRobot()  
    
51.         {  
    
52.             cmdvel_.linear.x = 0.0;  
    
53.             cmdvel_.angular.z = 0.0;  
    
54.             pub_.publish(cmdvel_);  
    
55.         }  
    
56. };  
    
57.   
    
58. SmartCarKeyboardTeleopNode* tbk;  
    
59. int kfd = 0;  
    
60. struct termios cooked, raw;  
    
61. bool done;  
    
62.   
    
63. int main(int argc, char** argv)  
    
64. {  
    
65.     ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);  
    
66.     SmartCarKeyboardTeleopNode tbk;  
    
67.       
    
68.     boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));  
    
69.       
    
70.     ros::spin();  
    
71.       
    
72.     t.interrupt();  
    
73.     t.join();  
    
74.     tbk.stopRobot();  
    
75.     tcsetattr(kfd, TCSANOW, &cooked);  
    
76.       
    
77.     return(0);  
    
78. }  
    
79.   
    
80. void SmartCarKeyboardTeleopNode::keyboardLoop()  
    
81. {  
    
82.     char c;  
    
83.     double max_tv = walk_vel_;  
    
84.     double max_rv = yaw_rate_;  
    
85.     bool dirty = false;  
    
86.     int speed = 0;  
    
87.     int turn = 0;  
    
88.       
    
89.     // get the console in raw mode  
    
90.     tcgetattr(kfd, &cooked);  
    
91.     memcpy(&raw, &cooked, sizeof(struct termios));  
    
92.     raw.c_lflag &=~ (ICANON | ECHO);  
    
93.     raw.c_cc[VEOL] = 1;  
    
94.     raw.c_cc[VEOF] = 2;  
    
95.     tcsetattr(kfd, TCSANOW, &raw);  
    
96.       
    
97.     puts("Reading from keyboard");  
    
98.     puts("Use WASD keys to control the robot");  
    
99.     puts("Press Shift to move faster");  
    
100.       
     
101.     struct pollfd ufd;  
     
102.     ufd.fd = kfd;  
     
103.     ufd.events = POLLIN;  
     
104.       
     
105.     for(;;)  
     
106.     {  
     
107.         boost::this_thread::interruption_point();  
     
108.          
     
109.         // get the next event from the keyboard  
     
110.         int num;  
     
111.          
     
112.         if ((num = poll(&ufd, 1, 250)) < 0)  
     
113.         {  
     
114.             perror("poll():");  
     
115.             return;  
     
116.         }  
     
117.         else if(num > 0)  
     
118.         {  
     
119.             if(read(kfd, &c, 1) < 0)  
     
120.             {  
     
121.                 perror("read():");  
     
122.                 return;  
     
123.             }  
     
124.         }  
     
125.         else  
     
126.         {  
     
127.             if (dirty == true)  
     
128.             {  
     
129.                 stopRobot();  
     
130.                 dirty = false;  
     
131.             }  
     
132.               
     
133.             continue;  
     
134.         }  
     
135.          
     
136.         switch(c)  
     
137.         {  
     
138.             case KEYCODE_W:  
     
139.                 max_tv = walk_vel_;  
     
140.                 speed = 1;  
     
141.                 turn = 0;  
     
142.                 dirty = true;  
     
143.                 break;  
     
144.             case KEYCODE_S:  
     
145.                 max_tv = walk_vel_;  
     
146.                 speed = -1;  
     
147.                 turn = 0;  
     
148.                 dirty = true;  
     
149.                 break;  
     
150.             case KEYCODE_A:  
     
151.                 max_rv = yaw_rate_;  
     
152.                 speed = 0;  
     
153.                 turn = 1;  
     
154.                 dirty = true;  
     
155.                 break;  
     
156.             case KEYCODE_D:  
     
157.                 max_rv = yaw_rate_;  
     
158.                 speed = 0;  
     
159.                 turn = -1;  
     
160.                 dirty = true;  
     
161.                 break;  
     
162.                   
     
163.             case KEYCODE_W_CAP:  
     
164.                 max_tv = run_vel_;  
     
165.                 speed = 1;  
     
166.                 turn = 0;  
     
167.                 dirty = true;  
     
168.                 break;  
     
169.             case KEYCODE_S_CAP:  
     
170.                 max_tv = run_vel_;  
     
171.                 speed = -1;  
     
172.                 turn = 0;  
     
173.                 dirty = true;  
     
174.                 break;  
     
175.             case KEYCODE_A_CAP:  
     
176.                 max_rv = yaw_rate_run_;  
     
177.                 speed = 0;  
     
178.                 turn = 1;  
     
179.                 dirty = true;  
     
180.                 break;  
     
181.             case KEYCODE_D_CAP:  
     
182.                 max_rv = yaw_rate_run_;  
     
183.                 speed = 0;  
     
184.                 turn = -1;  
     
185.                 dirty = true;  
     
186.                 break;               
     
187.             default:  
     
188.                 max_tv = walk_vel_;  
     
189.                 max_rv = yaw_rate_;  
     
190.                 speed = 0;  
     
191.                 turn = 0;  
     
192.                 dirty = false;  
     
193.         }  
     
194.         cmdvel_.linear.x = speed * max_tv;  
     
195.         cmdvel_.angular.z = turn * max_rv;  
     
196.         pub_.publish(cmdvel_);  
     
197.     }  
     
198. }  

复制代码

3、创新

        虽然很多机器人的键盘控制使用的都是C++编写的代码，但是考虑到python的强大，我们还是需要尝试使用python来编写程序。
        首先需要理解上面C++程序的流程。在上面的程序中，我们单独创建了一个线程来读取中断中的输入，然后根据输入发布不同的速度和角度消息。介于线程的概念还比较薄弱，在python中使用循环替代线程。然后需要考虑的只是如何使用python来处理中断中的输入字符，通过上网查找资料，发现使用的API和C++的基本是一致的。最终的程序如下：

1. [python] view plaincopy
   
2. #!/usr/bin/env python  
   
3. # -*- coding: utf-8 -*  
   
4.   
   
5. import  os  
   
6. import  sys  
   
7. import  tty, termios  
   
8. import roslib; roslib.load_manifest('smartcar_teleop')  
   
9. import rospy  
   
10. from geometry_msgs.msg import Twist  
    
11. from std_msgs.msg import String  
    
12.   
    
13. # 全局变量  
    
14. cmd = Twist()  
    
15. pub = rospy.Publisher('cmd_vel', Twist)  
    
16.   
    
17. def keyboardLoop():  
    
18.     #初始化  
    
19.     rospy.init_node('smartcar_teleop')  
    
20.     rate = rospy.Rate(rospy.get_param('~hz', 1))  
    
21.   
    
22.     #速度变量  
    
23.     walk_vel_ = rospy.get_param('walk_vel', 0.5)  
    
24.     run_vel_ = rospy.get_param('run_vel', 1.0)  
    
25.     yaw_rate_ = rospy.get_param('yaw_rate', 1.0)  
    
26.     yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)  
    
27.   
    
28.     max_tv = walk_vel_  
    
29.     max_rv = yaw_rate_  
    
30.   
    
31.     #显示提示信息  
    
32.     print "Reading from keyboard"  
    
33.     print "Use WASD keys to control the robot"  
    
34.     print "Press Caps to move faster"  
    
35.     print "Press q to quit"  
    
36.   
    
37.     #读取按键循环  
    
38.     while not rospy.is_shutdown():  
    
39.         fd = sys.stdin.fileno()  
    
40.         old_settings = termios.tcgetattr(fd)  
    
41.         #不产生回显效果  
    
42.         old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHO  
    
43.         try :  
    
44.             tty.setraw( fd )  
    
45.             ch = sys.stdin.read( 1 )  
    
46.         finally :  
    
47.             termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)  
    
48.   
    
49.         if ch == 'w':  
    
50.             max_tv = walk_vel_  
    
51.             speed = 1  
    
52.             turn = 0  
    
53.         elif ch == 's':  
    
54.             max_tv = walk_vel_  
    
55.             speed = -1  
    
56.             turn = 0  
    
57.         elif ch == 'a':  
    
58.             max_rv = yaw_rate_  
    
59.             speed = 0  
    
60.             turn = 1  
    
61.         elif ch == 'd':  
    
62.             max_rv = yaw_rate_  
    
63.             speed = 0  
    
64.             turn = -1  
    
65.         elif ch == 'W':  
    
66.             max_tv = run_vel_  
    
67.             speed = 1  
    
68.             turn = 0  
    
69.         elif ch == 'S':  
    
70.             max_tv = run_vel_  
    
71.             speed = -1  
    
72.             turn = 0  
    
73.         elif ch == 'A':  
    
74.             max_rv = yaw_rate_run_  
    
75.             speed = 0  
    
76.             turn = 1  
    
77.         elif ch == 'D':  
    
78.             max_rv = yaw_rate_run_  
    
79.             speed = 0  
    
80.             turn = -1  
    
81.         elif ch == 'q':  
    
82.             exit()  
    
83.         else:  
    
84.             max_tv = walk_vel_  
    
85.             max_rv = yaw_rate_  
    
86.             speed = 0  
    
87.             turn = 0  
    
88.   
    
89.         #发送消息  
    
90.         cmd.linear.x = speed * max_tv;  
    
91.         cmd.angular.z = turn * max_rv;  
    
92.         pub.publish(cmd)  
    
93.         rate.sleep()  
    
94.         #停止机器人  
    
95.         stop_robot();  
    
96.   
    
97. def stop_robot():  
    
98.     cmd.linear.x = 0.0  
    
99.     cmd.angular.z = 0.0  
    
100.     pub.publish(cmd)  
     
101.   
     
102. if __name__ == '__main__':  
     
103.     try:  
     
104.         keyboardLoop()  
     
105.     except rospy.ROSInterruptException:  
     
106.         pass  

复制代码

四、节点关系图