A-A+

ROS探索总结(八)——键盘控制

2013年07月12日 ROS 暂无评论 阅读 80,752 次

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

一、创建控制包

        首先,我们为键盘控制单独建立一个包:
  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文件运行:
        在rviz中看一下机器人是不是动起来了!

三、加入键盘控制

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

1、移植

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

2、复用

       因为代码是我们直接复制过来的,其中有很多与之前erratic机器人相关的变量,我们把代码稍作修改,变成自己机器人可读性较强的代码。
  1. #include
  2. #include
  3. #include
  4. #include
  5. #include
  6. #include
  7. #include
  8. #include
  9. #include
  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("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

3、创新

        虽然很多机器人的键盘控制使用的都是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
        希望大家一同学习,共同进步!

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

欢迎大家转载我的文章。

转载请注明:转自古-月

http://blog.csdn.net/hcx25909

欢迎继续关注我的博客

标签:

给我留言

Copyright © ExBot易科机器人实验室 保留所有权利.   Theme   Robin modified by poyoten

用户登录

分享到: