ROS键盘控制机器人教程.docx
- 文档编号:8076403
- 上传时间:2023-01-28
- 格式:DOCX
- 页数:13
- 大小:264.10KB
ROS键盘控制机器人教程.docx
《ROS键盘控制机器人教程.docx》由会员分享,可在线阅读,更多相关《ROS键盘控制机器人教程.docx(13页珍藏版)》请在冰豆网上搜索。
ROS键盘控制机器人教程
一、创建控制包
首先,我们为键盘控制单独建立一个包:
roscreate-pkgsmartcar_teleoprospygeometry_msgsstd_msgsroscpp
rosmake
二、简单的消息发布
在机器人仿真中,主要控制机器人移动的就是在机器人仿真中,主要控制机器人移动的就是Twist()结构,如果我们编程将这个结构通过程序发布成topic,自然就可以控制机器人了。
我们先用简单的python来尝试一下。
之前的模拟中,我们使用的都是在命令行下进行的消息发布,现在我们需要把这些命令转换成python代码,封装到一个单独的节点中去。
针对之前的命令行,我们可以很简单的在smartcar_teleop/scripts文件夹下编写如下的控制代码:
#!
/usr/bin/envpython
importroslib;roslib.load_manifest('smartcar_teleop')
importrospy
fromgeometry_msgs.msgimportTwist
fromstd_msgs.msgimportString
classTeleop:
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
whilenotrospy.is_shutdown():
str="helloworld%s"%rospy.get_time()
rospy.loginfo(str)
pub.publish(self.cmd)
rate.sleep()
if__name__=='__main__':
Teleop()
python代码在ROS重视不需要编译的。
先运行之前教程中用到的smartcar机器人,在rviz中进行显示,然后新建终端,输入如下命令:
rosrunsmartcar_teleopteleop.py
也可以建立一个launch文件运行:
在rviz中看一下机器人是不是动起来了!
三、加入键盘控制
当然前边的程序不是我们要的,我们需要的键盘控制。
1、移植
因为ROS的代码具有很强的可移植性,所以用键盘控制的代码其实可以直接从其他机器人包中移植过来,在这里我主要参考的是erratic_robot,在这个机器人的代码中有一个erratic_teleop包,可以直接移植过来使用。
首先,我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下,然后修改CMakeLists.txt文件,将下列代码加入文件底部:
rosbuild_add_boost_directories()
rosbuild_add_executable(smartcar_keyboard_teleopsrc/keyboard.cpp)
target_link_libraries(smartcar_keyboard_teleopboost_thread)
编译完成后,运行smartcar模型。
重新打开一个终端,打开键盘控制节点:
在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。
效果如下图:
2、复用
因为代码是我们直接复制过来的,其中有很多与之前erratic机器人相关的变量,我们把代码稍作修改,变成自己机器人可读性较强的代码。
#include
#include
#include
#include
#include
#include
#include
#include
#include
#defineKEYCODE_W0x77
#defineKEYCODE_A0x61
#defineKEYCODE_S0x73
#defineKEYCODE_D0x64
#defineKEYCODE_A_CAP0x41
#defineKEYCODE_D_CAP0x44
#defineKEYCODE_S_CAP0x53
#defineKEYCODE_W_CAP0x57
classSmartCarKeyboardTeleopNode
{
private:
doublewalk_vel_;
doublerun_vel_;
doubleyaw_rate_;
doubleyaw_rate_run_;
geometry_msgs:
:
Twistcmdvel_;
ros:
:
NodeHandlen_;
ros:
:
Publisherpub_;
public:
SmartCarKeyboardTeleopNode()
{
pub_=n_.advertise : Twist>("cmd_vel",1); ros: : NodeHandlen_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(){} voidkeyboardLoop(); voidstopRobot() { cmdvel_.linear.x=0.0; cmdvel_.angular.z=0.0; pub_.publish(cmdvel_); } }; SmartCarKeyboardTeleopNode*tbk; intkfd=0; structtermioscooked,raw; booldone; intmain(intargc,char**argv) { ros: : init(argc,argv,"tbk",ros: : init_options: : AnonymousName|ros: : init_options: : NoSigintHandler); SmartCarKeyboardTeleopNodetbk; boost: : threadt=boost: : thread(boost: : bind(&SmartCarKeyboardTeleopNode: : keyboardLoop,&tbk)); ros: : spin(); t.interrupt(); t.join(); tbk.stopRobot(); tcsetattr(kfd,TCSANOW,&cooked); return(0); } voidSmartCarKeyboardTeleopNode: : keyboardLoop() { charc; doublemax_tv=walk_vel_; doublemax_rv=yaw_rate_; booldirty=false; intspeed=0; intturn=0; //gettheconsoleinrawmode tcgetattr(kfd,&cooked); memcpy(&raw,&cooked,sizeof(structtermios)); raw.c_lflag&=~(ICANON|ECHO); raw.c_cc[VEOL]=1; raw.c_cc[VEOF]=2; tcsetattr(kfd,TCSANOW,&raw); puts("Readingfromkeyboard"); puts("UseWASDkeystocontroltherobot"); puts("PressShifttomovefaster"); structpollfdufd; ufd.fd=kfd; ufd.events=POLLIN; for(;;) { boost: : this_thread: : interruption_point(); //getthenexteventfromthekeyboard intnum; if((num=poll(&ufd,1,250))<0) { perror("poll(): "); return; } elseif(num>0) { if(read(kfd,&c,1)<0) { perror("read(): "); return; } } else { if(dirty==true) { stopRobot(); dirty=false; } continue; } switch(c) { caseKEYCODE_W: max_tv=walk_vel_; speed=1; turn=0; dirty=true; break; caseKEYCODE_S: max_tv=walk_vel_; speed=-1; turn=0; dirty=true; break; caseKEYCODE_A: max_rv=yaw_rate_; speed=0; turn=1; dirty=true; break; caseKEYCODE_D: max_rv=yaw_rate_; speed=0; turn=-1; dirty=true; break; caseKEYCODE_W_CAP: max_tv=run_vel_; speed=1; turn=0; dirty=true; break; caseKEYCODE_S_CAP: max_tv=run_vel_; speed=-1; turn=0; dirty=true; break; caseKEYCODE_A_CAP: max_rv=yaw_rate_run_; speed=0; turn=1; dirty=true; break; caseKEYCODE_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_); } } 3、创新 虽然很多机器人的键盘控制使用的都是C++编写的代码,但是考虑到python的强大,我们还是需要尝试使用python来编写程序。 首先需要理解上面C++程序的流程。 在上面的程序中,我们单独创建了一个线程来读取中断中的输入,然后根据输入发布不同的速度和角度消息。 介于线程的概念还比较薄弱,在python中使用循环替代线程。 然后需要考虑的只是如何使用python来处理中断中的输入字符,通过上网查找资料,发现使用的API和C++的基本是一致的。 最终的程序如下: #! /usr/bin/envpython #-*-coding: utf-8-* importos importsys importtty,termios importroslib;roslib.load_manifest('smartcar_teleop') importrospy fromgeometry_msgs.msgimportTwist fromstd_msgs.msgimportString #全局变量 cmd=Twist() pub=rospy.Publisher('cmd_vel',Twist) defkeyboardLoop(): #初始化 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"Readingfromkeyboard" print"UseWASDkeystocontroltherobot" print"PressCapstomovefaster" print"Pressqtoquit" #读取按键循环 whilenotrospy.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) ifch=='w': max_tv=walk_vel_ speed=1 turn=0 elifch=='s': max_tv=walk_vel_ speed=-1 turn=0 elifch=='a': max_rv=yaw_rate_ speed=0 turn=1 elifch=='d': max_rv=yaw_rate_ speed=0 turn=-1 elifch=='W': max_tv=run_vel_ speed=1 turn=0 elifch=='S': max_tv=run_vel_ speed=-1 turn=0 elifch=='A': max_rv=yaw_rate_run_ speed=0 turn=1 elifch=='D': max_rv=yaw_rate_run_ speed=0 turn=-1 elifch=='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(); defstop_robot(): cmd.linear.x=0.0 cmd.angular.z=0.0 pub.publish(cmd) if__name__=='__main__': try: keyboardLoop() exceptrospy.ROSInterruptException: pass 四、节点关系图
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- ROS 键盘 控制 机器人 教程