1. rosmake smartcar_description --pre-clean

1. <arg name="gui" default="false" />

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

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文件夹下编写如下的控制代码：

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. class Teleop:
7. def __init__(self):
8. pub = rospy.Publisher('cmd_vel', Twist)
9. rospy.init_node('smartcar_teleop')
10. rate = rospy.Rate(rospy.get_param('~hz', 1))
11. self.cmd = None
12. cmd = Twist()
13. cmd.linear.x = 0.2
14. cmd.linear.y = 0
15. cmd.linear.z = 0
16. cmd.angular.z = 0
17. cmd.angular.z = 0
18. cmd.angular.z = 0.5
19. self.cmd = cmd
20. while not rospy.is_shutdown():
21. str = "hello world %s" % rospy.get_time()
22. rospy.loginfo(str)
23. pub.publish(self.cmd)
24. rate.sleep()
25. if __name__ == '__main__':Teleop()

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

1. rosrun smartcar_teleop teleop.py

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

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中看一下机器人是不是动起来了！

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

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

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”键进行机器人的控制。效果如下图：

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

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

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

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

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

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

四、节点关系图

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

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

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

        首先将操作杆的接口插入电脑，然后在终端中输入：

1. ls /dev/input/

        显示如下：

        其中的js0就代表我们的操作杆。然后测试操作杆的操作是否有效，输入:

1. sudo jstest /dev/input/js0

        然后会在终端中显示操作杆的各个控制值的即时值，操作操作杆，如果每个按键和操作都有效，说明操作杆是正常的。最后在ROS中的节点里进行测试。打开joy节点：

1. rosrun joy joy_node

        再打开一个窗口，输入下面命令，查看数据:

1. rostopic echo joy

        操作操作杆，窗口下面的数据就开始刷新。

        从上面的测试中，我们发现在前后左右摇动操作杆时，相应改变的数据是Axes中0号和1号位的数据，也是我们最常用的数据，其他按键对应的位置也可以找到，编程的时候就是利用的这些数据位置，所以一定要找到每个按键的对应编号。

        ROS中已经为我们建立了操作杆的数据结构：

        我们主要用到的就是axes和buttons数据。最终的代码如下：

1. import roslib; roslib.load_manifest('smartcar_teleop')
2. import rospy
3. from sensor_msgs.msg import Joy
4. from geometry_msgs.msg import Twist
5. from std_msgs.msg import String
6. class Teleop:
7. def __init__(self):
8. rospy.init_node('smartcar_teleop_joy')
9. self.turn_scale = rospy.get_param('~turn_scale')
10. self.drive_scale = rospy.get_param('~drive_scale')
11. self.deadman_button = rospy.get_param('~deadman_button', 0)
12. self.cmd = None
13. cmd_pub = rospy.Publisher('cmd_vel', Twist)
14. announce_pub = rospy.Publisher('/smartcar/announce/teleops',
15. String, latch=True)
16. announce_pub.publish(rospy.get_namespace());
17. rospy.Subscriber("joy", Joy, self.callback)
18. rate = rospy.Rate(rospy.get_param('~hz', 20))
19. while not rospy.is_shutdown():
20. rate.sleep()
21. if self.cmd:
22. cmd_pub.publish(self.cmd)
23. def callback(self, data):
24. """ Receive joystick data, formulate Twist message. """
25. cmd = Twist()
26. cmd.linear.x = data.axes[1] * self.drive_scale
27. cmd.angular.z = data.axes[0] * self.turn_scale
28. if data.buttons[self.deadman_button] == 1:
29. self.cmd = cmd
30. else:
31. self.cmd = None
32. if __name__ == "__main__": Teleop()

三、机器人控制

        在rviz中打开我们的机器人模型，然后打开操作杆的控制节点：

1. roslaunch smartcar_display.rviz.launch
2. roslaunch smartcar_teleop teleop_joy.launch

        然后按住刹车键进行操作，机器人就可以开始移动了：

        在新终端中输入：

1. rostopic echo joy

         可以查看到实时的操作杆控制数据：

四、节点关系图

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

欢迎大家转载我的文章。

转载请注明：转自古-月

http://blog.csdn.net/hcx25909

欢迎继续关注我的博客