一、创建控制包

1 catkin_create-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp  
2 catkin_make

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

二、简单的消息发布

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

 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()

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

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

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、移植

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

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

keyboard.cpp代码如下：

  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 }

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

rosrun smartcar_teleop smartcar_keyboard_teleop

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

posted on 2017-05-03 18:48 林先森_007 阅读(3165) 评论(0) 编辑收藏举报

会员力量，点亮园子希望

刷新页面返回顶部