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

1 catkin_create-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp  
2 catkin_make

1 #!/usr/bin/env python
2 import roslib;
3 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 """docstring fos Teleop"""
10 def __init__(self):
11 pub = rospy.Publisher('cmd_vel',Twist)
12 rospy.init_node('smartcar_teleop')
13 rate = rospy.Rate(rospy.get_param('~hz',1))
14 self.cmd = None
15 
16 cmd =Twist()
17 cmd.linear.x = 0.2
18 cmd.linear.y = 0
19 cmd.linear.z = 0
20 
21 cmd.angular.x = 0
22 cmd.angular.y = 0
23 cmd.angular.z = 0.5
24 
25 self.cmd = cmd
26 
27 while not rospy.is_shutdown():
28 str = "Hello world %s" %rospy.get_time()
29 rospy.loginfo(str)
30 pub.publish(self.cmd)
31 rate.sleep()
32 if __name__ == "__main__":Teleop()

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

可以建立一个launch文件（teleop.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>

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

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 }

rosrun smartcar_teleop smartcar_keyboard_teleop