ROS探索总结(八)——键盘控制
来源:互联网 发布:网络神作小说 知乎 编辑:程序博客网 时间:2024/05/11 04:30
如果尝试过前面的例子,有没有感觉每次让机器人移动还要在终端里输入指令,这也太麻烦了,有没有办法通过键盘来控制机器人的移动呢?答案室当然的了。我研究了其他几个机器人键盘控制的代码,还是有所收获的,最后移植到了smartcar上,实验成功。
一、创建控制包
首先,我们为键盘控制单独建立一个包:
roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpprosmake
如果你已经忘记了怎么建立包,请参考:http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage
二、简单的消息发布
在机器人仿真中,主要控制机器人移动的就是 在机器人仿真中,主要控制机器人移动的就是Twist()结构,如果我们编程将这个结构通过程序发布成topic,自然就可以控制机器人了。我们先用简单的python来尝试一下。
之前的模拟中,我们使用的都是在命令行下进行的消息发布,现在我们需要把这些命令转换成python代码,封装到一个单独的节点中去。针对之前的命令行,我们可以很简单的在smartcar_teleop /scripts文件夹下编写如下的控制代码:
#!/usr/bin/env pythonimport roslib; roslib.load_manifest('smartcar_teleop')import rospyfrom geometry_msgs.msg import Twistfrom std_msgs.msg import Stringclass 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中进行显示,然后新建终端,输入如下命令:
rosrun smartcar_teleop teleop.py
也可以建立一个launch文件运行:
<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文件,将下列代码加入文件底部:
首先,我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下,然后修改CMakeLists.txt文件,将下列代码加入文件底部:
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机器人相关的变量,我们把代码稍作修改,变成自己机器人可读性较强的代码。
#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 0x57class SmartCarKeyboardTeleopNode{ private: double walk_vel_; double run_vel_; double yaw_rate_; double yaw_rate_run_; geometry_msgs::Twist cmdvel_; ros::NodeHandle n_; ros::Publisher 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("Press 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/simulator_gazebo/Tutorials/TeleopErraticSimulation
http://www.ros.org/wiki/simulator_gazebo/Tutorials/TeleopErraticSimulation
3、创新
虽然很多机器人的键盘控制使用的都是C++编写的代码,但是考虑到python的强大,我们还是需要尝试使用python来编写程序。
首先需要理解上面C++程序的流程。在上面的程序中,我们单独创建了一个线程来读取中断中的输入,然后根据输入发布不同的速度和角度消息。介于线程的概念还比较薄弱,在python中使用循环替代线程。然后需要考虑的只是如何使用python来处理中断中的输入字符,通过上网查找资料,发现使用的API和C++的基本是一致的。最终的程序如下:
首先需要理解上面C++程序的流程。在上面的程序中,我们单独创建了一个线程来读取中断中的输入,然后根据输入发布不同的速度和角度消息。介于线程的概念还比较薄弱,在python中使用循环替代线程。然后需要考虑的只是如何使用python来处理中断中的输入字符,通过上网查找资料,发现使用的API和C++的基本是一致的。最终的程序如下:
#!/usr/bin/env python# -*- coding: utf-8 -*import osimport sysimport tty, termiosimport roslib; roslib.load_manifest('smartcar_teleop')import rospyfrom geometry_msgs.msg import Twistfrom 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 "Press Caps to move faster" print "Press 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://nullege.com/codes/search/termios.ICANON
四、节点关系图
代码包我已上传:http://download.csdn.net/detail/hcx25909/5496381
希望大家一同学习,共同进步!
----------------------------------------------------------------
欢迎大家转载我的文章。
转载请注明:转自古-月
http://blog.csdn.net/hcx25909
欢迎继续关注我的博客
- ROS探索总结(八)——键盘控制
- ROS探索总结(八)——键盘控制
- ROS探索总结(七)(八)(九)——smartcar源码上传 键盘控制 操作杆控制
- ROS探索总结(九)——操作杆控制
- ROS探索总结(十)——语音控制
- ROS探索总结(九)——操作杆控制
- ROS探索总结(十)——语音控制
- ROS探索总结(一)——ROS简介
- ROS探索总结(二)——ROS总体框架
- ROS探索总结(三)——ROS新手教程
- ROS探索总结(一)——ROS简介
- ROS探索总结(二)——ROS总体框架
- ROS探索总结(三)——ROS新手教程
- ROS探索总结(一)——ROS简介
- ROS探索总结(二)——ROS总体框架
- ROS探索总结(一)——ROS简介
- ROS探索总结(二)——ROS总体框架
- ROS探索总结(三)——ROS新手教程
- 服务器模型特点对比
- 浅析 Java Thread.join()
- matlab读取sina股市数据
- jboss性能相关(二)
- LINUX-C成长之路(四):运算符
- ROS探索总结(八)——键盘控制
- 李开复:真正成功的公司源于伟大的创业理念
- Fatal signal 11问题的解决方法
- C#代码实现邮箱验证C#中及一些常用的正则表达式
- 使用java操作wavecom短信猫来发短信的方法
- 《windows核心编程》读后编码--创建线程
- 数据库初步了解
- kernel参数值设定
- 电脑里的所有播放器只能播放声音没有画面