程序博客网 > 韩子高网络剧第二部

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

来源：互联网 发布：韩子高网络剧第二部 编辑：程序博客网 时间：2024/05/16 23:38

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

一、创建控制包

        首先，我们为键盘控制单独建立一个包：
[plain] view plaincopy
  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 plaincopy
  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 plaincopy
  1. rosrun smartcar_teleop teleop.py  


        也可以建立一个launch文件运行：
[plain] view plaincopy
  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 plaincopy
  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 plaincopy
  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 plaincopy
  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

欢迎继续关注我的博客

0 0
韩子高网络剧第二部 韩子高网络剧第二部
原创粉丝点击
热门IT博客
公司电脑监控软件公司电脑监控软件
sap sqlsap sql
4g有信号无法连接网络
水印素材软件
淘宝外卖怎么入驻
淘宝人员考核标准
软件开发怎么学
免费房屋设计软件下载
融资融券系统源码
淘宝钻石等级买家
数据库存储过程是什么
淘宝网销售排行
天猫化妆品销售数据
淘宝网支持微信支付吗
连接linux的终端
java众数计算
linux count
parquet java api
查询加油站价格软件
安徽巨蟒渡劫事件知乎
热门问题 老师的惩罚 人脸识别 我在镇武司摸鱼那些年 重生之率土为王 我在大康的咸鱼生活 盘龙之生命进化 天生仙种 凡人之先天五行 春回大明朝 姑娘不必设防，我是瞎子 车缺电打不着火怎么办 车子电脑板坏了怎么办 汽车电脑板坏了怎么办? 小车三角臂坏了怎么办 贵阳摇号中签后怎么办 版面费发票丢了怎么办 当被实名举报了怎么办 玩游戏手老出汗怎么办 智慧树密码忘了怎么办 汽车还完贷款后怎么办 魅蓝note5耗电快怎么办 电脑开机键坏了怎么办 机箱开机键坏了怎么办 起诉后被告不来怎么办 40岁头发全白了怎么办 30岁头发全白了怎么办 领导总是找我茬怎么办 领导想把我整走怎么办 被公司降职降薪怎么办 工作中被降职了怎么办 高中毕业出国留学档案怎么办 汽车智能钥匙丢了怎么办 车子钥匙没电了怎么办 奔驰s级维修太贵怎么办 白酒喝多了头晕怎么办 喝醉酒了胃难受怎么办 玩梦三国2卡怎么办 退税单写错了怎么办 淘宝扣了12分怎么办 淘宝店扣了12分怎么办 被淘宝扣12分怎么办 没用完的佛香怎么办 我在寺庙拍照了怎么办 卸载完cad黑屏了怎么办 玉坠缝隙里脏了怎么办 购房公摊面积大怎么办 贴皮家具起皮怎么办 酱牛肉有点酸了怎么办 疯马皮沾油变黑怎么办 疯马皮沾上油渍怎么办 防盗窗下雨很吵怎么办

程序博客网，程序员的互联网技术博客家园。csdn论坛精品 msdn技术资料都在这里