ROS探索总结（八）——键盘控制

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

一、创建控制包

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

[plain]  view plain copy

1. roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp  
2. rosmake  

        如果你已经忘记了怎么建立包，请参考：http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage

二、简单的消息发布

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

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

[python]  view plain copy

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

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

[plain]  view plain copy

1. rosrun smartcar_teleop teleop.py  

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

[plain]  view plain copy

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

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

三、加入键盘控制

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

1、移植

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

[plain]  view plain copy

1. rosbuild_add_boost_directories()  
2. rosbuild_add_executable(smartcar_keyboard_teleop src/keyboard.cpp)  
3. target_link_libraries(smartcar_keyboard_teleop boost_thread)  

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

        

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

2、复用 

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

[cpp]  view plain copy

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

参考链接：http://ros.org/wiki/turtlebot_teleop/Tutorials/Teleoperation
                  http://www.ros.org/wiki/simulator_gazebo/Tutorials/TeleopErraticSimulation

3、创新

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

[python]  view plain copy

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

参考链接：http://blog.csdn.net/marising/article/details/3173848
                  http://nullege.com/codes/search/termios.ICANON

四、节点关系图

        代码包我已上传：http://download.csdn.net/detail/hcx25909/5496381

        希望大家一同学习，共同进步！

----------------------------------------------------------------

欢迎大家转载我的文章。

转载请注明：转自古-月

http://blog.csdn.net/hcx25909

欢迎继续关注我的博客