首页 资讯 应用 高压 设计 行业 低压 电路图 关于

机器人

旗下栏目: 物联网 机器人 新技术 仪器仪表

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

机器人 | 发布时间:2017-10-12 | 人气: | #评论# |本文关键字:ROS,ROS探索,键盘控制
摘要:如果尝试过前面的例子,有没有感觉每次让机器人移动还要在终端里输入指令,这也太麻烦了,有没有办法通过键盘来控制机器人的移动呢?答案室当然的了。我研究了其他几个机器人

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

一、创建控制包

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

roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp  

  1. 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.  
  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中进行显示,然后新建终端,输入如下命令:

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

三、加入键盘控制

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

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模型。重新打开一个终端,打开键盘控制节点:

2codes.png

       

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

1507815741842390.png

2、复用

       因为代码是我们直接复制过来的,其中有很多与之前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.  
  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

责任编辑:电气自动化网
首页 | 资讯 | 应用 | 高压 | 设计 | 行业 | 低压 | 电路图 | 关于

Copyright 2017-2018 电气自动化网 版权所有 辽ICP备17010593号-1

电脑版 | 移动版

Ctrl+D 将本页面保存为书签,全面了解最新资讯,方便快捷。