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

pengpengpang

如果尝试过前面的例子，有没有感觉每次让机器人移动还要在终端里输入指令，这也太麻烦了，有没有办法通过键盘来控制机器人的移动呢？答案室当然的了。我研究了其他几个机器人键盘控制的代码，还是有所收获的，最后移植到了smartcar上，实验成功。 一、创建控制包        首先，我们为键盘控制单独建立一个包：[plain] http://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="view plain">view plainhttp://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="copy">copy

• roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
• rosmake

如果你已经忘记了怎么建立包，请参考：http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage 二、简单的消息发布        在机器人仿真中，主要控制机器人移动的就是        在机器人仿真中，主要控制机器人移动的就是Twist()结构，如果我们编程将这个结构通过程序发布成topic，自然就可以控制机器人了。我们先用简单的python来尝试一下。         之前的模拟中，我们使用的都是在命令行下进行的消息发布，现在我们需要把这些命令转换成python代码，封装到一个单独的节点中去。针对之前的命令行，我们可以很简单的在smartcar_teleop /scripts文件夹下编写如下的控制代码：[python] http://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="view plain">view plainhttp://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="copy">copy

• #!/usr/bin/env python
• import roslib; roslib.load_manifest('smartcar_teleop')
• import rospy
• from geometry_msgs.msg import Twist
• from std_msgs.msg import String
• class Teleop:
•     def __init__(self):
•         pub = rospy.Publisher('cmd_vel', Twist)
•         rospy.init_node('smartcar_teleop')
•         rate = rospy.Rate(rospy.get_param('~hz', 1))
•         self.cmd = None
•         cmd = Twist()
•         cmd.linear.x = 0.2
•         cmd.linear.y = 0
•         cmd.linear.z = 0
•         cmd.angular.z = 0
•         cmd.angular.z = 0
•         cmd.angular.z = 0.5
•         self.cmd = cmd
•         while not rospy.is_shutdown():
•             str = "hello world %s" % rospy.get_time()
•             rospy.loginfo(str)
•             pub.publish(self.cmd)
•             rate.sleep()
• if __name__ == '__main__':Teleop()

        python代码在ROS重视不需要编译的。先运行之前教程中用到的smartcar机器人，在rviz中进行显示，然后新建终端，输入如下命令：[plain] http://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="view plain">view plainhttp://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="copy">copy

• rosrun smartcar_teleop teleop.py

         也可以建立一个launch文件运行：[plain] http://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="view plain">view plainhttp://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="copy">copy

• <launch>
•   <arg name="cmd_topic" default="cmd_vel" />
•   <node pkg="smartcar_teleop" type="teleop.py" name="smartcar_teleop">
•     <remap from="cmd_vel" to="$(arg cmd_topic)" />
•   </node>
• </launch>

        在rviz中看一下机器人是不是动起来了！ 三、加入键盘控制        当然前边的程序不是我们要的，我们需要的键盘控制。 1、移植        因为ROS的代码具有很强的可移植性，所以用键盘控制的代码其实可以直接从其他机器人包中移植过来，在这里我主要参考的是erratic_robot，在这个机器人的代码中有一个erratic_teleop包，可以直接移植过来使用。        首先，我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下，然后修改CMakeLists.txt文件，将下列代码加入文件底部：[plain] http://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="view plain">view plainhttp://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="copy">copy

• rosbuild_add_boost_directories()
• rosbuild_add_executable(smartcar_keyboard_teleop src/keyboard.cpp)
• target_link_libraries(smartcar_keyboard_teleop boost_thread)

        编译完成后，运行smartcar模型。重新打开一个终端，打开键盘控制节点：                在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。效果如下图： 2、复用        因为代码是我们直接复制过来的，其中有很多与之前erratic机器人相关的变量，我们把代码稍作修改，变成自己机器人可读性较强的代码。[cpp] http://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="view plain">view plainhttp://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="copy">copy

• #include <termios.h>
• #include <signal.h>
• #include <math.h>
• #include <stdio.h>
• #include <stdlib.h>
• #include <sys/poll.h>
• #include <boost/thread/thread.hpp>
• #include <ros/ros.h>
• #include <geometry_msgs/Twist.h>
• #define KEYCODE_W 0x77
• #define KEYCODE_A 0x61
• #define KEYCODE_S 0x73
• #define KEYCODE_D 0x64
• #define KEYCODE_A_CAP 0x41
• #define KEYCODE_D_CAP 0x44
• #define KEYCODE_S_CAP 0x53
• #define KEYCODE_W_CAP 0x57
• class SmartCarKeyboardTeleopNode
• {
•     private:
•         double walk_vel_;
•         double run_vel_;
•         double yaw_rate_;
•         double yaw_rate_run_;
•         geometry_msgs::Twist cmdvel_;
•         ros::NodeHandle n_;
•         ros:ublisher pub_;
•     public:
•         SmartCarKeyboardTeleopNode()
•         {
•             pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
•             ros::NodeHandle n_private("~");
•             n_private.param("walk_vel", walk_vel_, 0.5);
•             n_private.param("run_vel", run_vel_, 1.0);
•             n_private.param("yaw_rate", yaw_rate_, 1.0);
•             n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
•         }
•         ~SmartCarKeyboardTeleopNode() { }
•         void keyboardLoop();
•         void stopRobot()
•         {
•             cmdvel_.linear.x = 0.0;
•             cmdvel_.angular.z = 0.0;
•             pub_.publish(cmdvel_);
•         }
• };
• SmartCarKeyboardTeleopNode* tbk;
• int kfd = 0;
• struct termios cooked, raw;
• bool done;
• int main(int argc, char** argv)
• {
•     ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
•     SmartCarKeyboardTeleopNode tbk;
•     boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));
•     ros::spin();
•     t.interrupt();
•     t.join();
•     tbk.stopRobot();
•     tcsetattr(kfd, TCSANOW, &cooked);
•     return(0);
• }
• void SmartCarKeyboardTeleopNode::keyboardLoop()
• {
•     char c;
•     double max_tv = walk_vel_;
•     double max_rv = yaw_rate_;
•     bool dirty = false;
•     int speed = 0;
•     int turn = 0;
•     // get the console in raw mode
•     tcgetattr(kfd, &cooked);
•     memcpy(&raw, &cooked, sizeof(struct termios));
•     raw.c_lflag &=~ (ICANON | ECHO);
•     raw.c_cc[VEOL] = 1;
•     raw.c_cc[VEOF] = 2;
•     tcsetattr(kfd, TCSANOW, &raw);
•     puts("Reading from keyboard");
•     puts("Use WASD keys to control the robot");
•     puts("ress Shift to move faster");
•     struct pollfd ufd;
•     ufd.fd = kfd;
•     ufd.events = POLLIN;
•     for(;;)
•     {
•         boost::this_thread::interruption_point();
•         // get the next event from the keyboard
•         int num;
•         if ((num = poll(&ufd, 1, 250)) < 0)
•         {
•             perror("poll():");
•             return;
•         }
•         else if(num > 0)
•         {
•             if(read(kfd, &c, 1) < 0)
•             {
•                 perror("read():");
•                 return;
•             }
•         }
•         else
•         {
•             if (dirty == true)
•             {
•                 stopRobot();
•                 dirty = false;
•             }
•             continue;
•         }
•         switch(c)
•         {
•             case KEYCODE_W:
•                 max_tv = walk_vel_;
•                 speed = 1;
•                 turn = 0;
•                 dirty = true;
•                 break;
•             case KEYCODE_S:
•                 max_tv = walk_vel_;
•                 speed = -1;
•                 turn = 0;
•                 dirty = true;
•                 break;
•             case KEYCODE_A:
•                 max_rv = yaw_rate_;
•                 speed = 0;
•                 turn = 1;
•                 dirty = true;
•                 break;
•             case KEYCODE_D:
•                 max_rv = yaw_rate_;
•                 speed = 0;
•                 turn = -1;
•                 dirty = true;
•                 break;
•             case KEYCODE_W_CAP:
•                 max_tv = run_vel_;
•                 speed = 1;
•                 turn = 0;
•                 dirty = true;
•                 break;
•             case KEYCODE_S_CAP:
•                 max_tv = run_vel_;
•                 speed = -1;
•                 turn = 0;
•                 dirty = true;
•                 break;
•             case KEYCODE_A_CAP:
•                 max_rv = yaw_rate_run_;
•                 speed = 0;
•                 turn = 1;
•                 dirty = true;
•                 break;
•             case KEYCODE_D_CAP:
•                 max_rv = yaw_rate_run_;
•                 speed = 0;
•                 turn = -1;
•                 dirty = true;
•                 break;
•             default:
•                 max_tv = walk_vel_;
•                 max_rv = yaw_rate_;
•                 speed = 0;
•                 turn = 0;
•                 dirty = false;
•         }
•         cmdvel_.linear.x = speed * max_tv;
•         cmdvel_.angular.z = turn * max_rv;
•         pub_.publish(cmdvel_);
•     }
• }

参考链接：http://ros.org/wiki/turtlebot_teleop/Tutorials/Teleoperation                  http://www.ros.org/wiki/simulato ... opErraticSimulation3、创新        虽然很多机器人的键盘控制使用的都是C++编写的代码，但是考虑到python的强大，我们还是需要尝试使用python来编写程序。        首先需要理解上面C++程序的流程。在上面的程序中，我们单独创建了一个线程来读取中断中的输入，然后根据输入发布不同的速度和角度消息。介于线程的概念还比较薄弱，在python中使用循环替代线程。然后需要考虑的只是如何使用python来处理中断中的输入字符，通过上网查找资料，发现使用的API和C++的基本是一致的。最终的程序如下：[python] http://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="view plain">view plainhttp://static.blog.csdn.net/scripts/SyntaxHighlighter/styles/images/defa...); background-color: inherit; border: none; padding: 1px; margin: 0px 10px 0px 0px; font-size: 9px; display: inline-block; width: 16px; height: 16px; text-indent: -2000px; background-position: 0% 0%; background-repeat: no-repeat no-repeat;" title="copy">copy

• #!/usr/bin/env python
• # -*- coding: utf-8 -*
• import  os
• import  sys
• import  tty, termios
• import roslib; roslib.load_manifest('smartcar_teleop')
• import rospy
• from geometry_msgs.msg import Twist
• from std_msgs.msg import String
• # 全局变量
• cmd = Twist()
• pub = rospy.Publisher('cmd_vel', Twist)
• def keyboardLoop():
•     #初始化
•     rospy.init_node('smartcar_teleop')
•     rate = rospy.Rate(rospy.get_param('~hz', 1))
•     #速度变量
•     walk_vel_ = rospy.get_param('walk_vel', 0.5)
•     run_vel_ = rospy.get_param('run_vel', 1.0)
•     yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
•     yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)
•     max_tv = walk_vel_
•     max_rv = yaw_rate_
•     #显示提示信息
•     print "Reading from keyboard"
•     print "Use WASD keys to control the robot"
•     print "ress Caps to move faster"
•     print "ress q to quit"
•     #读取按键循环
•     while not rospy.is_shutdown():
•         fd = sys.stdin.fileno()
•         old_settings = termios.tcgetattr(fd)
•         #不产生回显效果
•         old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHO
•         try :
•             tty.setraw( fd )
•             ch = sys.stdin.read( 1 )
•         finally :
•             termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
•         if ch == 'w':
•             max_tv = walk_vel_
•             speed = 1
•             turn = 0
•         elif ch == 's':
•             max_tv = walk_vel_
•             speed = -1
•             turn = 0
•         elif ch == 'a':
•             max_rv = yaw_rate_
•             speed = 0
•             turn = 1
•         elif ch == 'd':
•             max_rv = yaw_rate_
•             speed = 0
•             turn = -1
•         elif ch == 'W':
•             max_tv = run_vel_
•             speed = 1
•             turn = 0
•         elif ch == 'S':
•             max_tv = run_vel_
•             speed = -1
•             turn = 0
•         elif ch == 'A':
•             max_rv = yaw_rate_run_
•             speed = 0
•             turn = 1
•         elif ch == 'D':
•             max_rv = yaw_rate_run_
•             speed = 0
•             turn = -1
•         elif ch == 'q':
•             exit()
•         else:
•             max_tv = walk_vel_
•             max_rv = yaw_rate_
•             speed = 0
•             turn = 0
•         #发送消息
•         cmd.linear.x = speed * max_tv;
•         cmd.angular.z = turn * max_rv;
•         pub.publish(cmd)
•         rate.sleep()
•         #停止机器人
•         stop_robot();
• def stop_robot():
•     cmd.linear.x = 0.0
•     cmd.angular.z = 0.0
•     pub.publish(cmd)
• if __name__ == '__main__':
•     try:
•         keyboardLoop()
•     except rospy.ROSInterruptException:
•         pass

参考链接：http://blog.csdn.net/marising/article/details/3173848                  http://nullege.com/codes/search/termios.ICANON 四、节点关系图        代码包我已上传：http://download.csdn.net/detail/hcx25909/5496381        希望大家一同学习，共同进步！来源：转自古-月的博客