ROS键盘控制机器人教程文档格式.docx
《ROS键盘控制机器人教程文档格式.docx》由会员分享，可在线阅读，更多相关《ROS键盘控制机器人教程文档格式.docx（13页珍藏版）》请在冰豆网上搜索。
cmd.linear.y=0
cmd.linear.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文件运行：
<
launch>
<
argname="
cmd_topic"
default="
cmd_vel"
/>
nodepkg="
smartcar_teleop"
type="
teleop.py"
name="
>
remapfrom="
to="
$（argcmd_topic）"
/node>
/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<
termios.h>
signal.h>
math.h>
stdio.h>
stdlib.h>
sys/poll.h>
boost/thread/thread.hpp>
ros/ros.h>
geometry_msgs/Twist.h>
#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_;
Publisherpub_;
public:
SmartCarKeyboardTeleopNode（）
{
pub_=n_.advertise<
geometry_msgs:
Twist>
（"
1）;
NodeHandlen_private（"
~"
）;
n_private.param（"
walk_vel"
walk_vel_,0.5）;
run_vel"
run_vel_,1.0）;
yaw_rate"
yaw_rate_,1.0）;
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）
init（argc,argv,"
tbk"
ros:
init_options:
AnonymousName|ros:
NoSigintHandler）;
SmartCarKeyboardTeleopNodetbk;
boost:
threadt=boost:
thread（boost:
bind（&
SmartCarKeyboardTeleopNode:
keyboardLoop,&
tbk））;
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,&
memcpy（&
raw,&
cooked,sizeof（structtermios））;
raw.c_lflag&
=~（ICANON|ECHO）;
raw.c_cc[VEOL]=1;
raw.c_cc[VEOF]=2;
raw）;
puts（"
Readingfromkeyboard"
UseWASDkeystocontroltherobot"
PressShifttomovefaster"
structpollfdufd;
ufd.fd=kfd;
ufd.events=POLLIN;
for（;
;
this_thread:
interruption_point（）;
//getthenexteventfromthekeyboard
intnum;
if（（num=poll（&
ufd,1,250））<
0）
perror（"
poll（）:
"
return;
elseif（num>
if（read（kfd,&
c,1）<
read（）:
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:
speed=-1;
caseKEYCODE_A:
max_rv=yaw_rate_;
speed=0;
turn=1;
caseKEYCODE_D:
turn=-1;
caseKEYCODE_W_CAP:
max_tv=run_vel_;
caseKEYCODE_S_CAP:
caseKEYCODE_A_CAP:
max_rv=yaw_rate_run_;
caseKEYCODE_D_CAP:
default:
cmdvel_.linear.x=speed*max_tv;
cmdvel_.angular.z=turn*max_rv;
3、创新
虽然很多机器人的键盘控制使用的都是C++编写的代码，但是考虑到python的强大，我们还是需要尝试使用python来编写程序。
首先需要理解上面C++程序的流程。
在上面的程序中，我们单独创建了一个线程来读取中断中的输入，然后根据输入发布不同的速度和角度消息。
介于线程的概念还比较薄弱，在python中使用循环替代线程。
然后需要考虑的只是如何使用python来处理中断中的输入字符，通过上网查找资料，发现使用的API和C++的基本是一致的。
最终的程序如下：
#-*-coding:
utf-8-*
importos
importsys
importtty,termios
#全局变量
cmd=Twist（）
pub=rospy.Publisher（'
defkeyboardLoop（）:
#初始化
#速度变量
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'
yaw_rate_run_=rospy.get_param（'
yaw_rate_run'
1.5）
max_tv=walk_vel_
max_rv=yaw_rate_
#显示提示信息
print"
print"
PressCapstomovefaster"
Pressqtoquit"
#读取按键循环
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'
speed=1
turn=0
elifch=='
s'
speed=-1
a'
speed=0
turn=1
d'
turn=-1
W'
max_tv=run_vel_
S'
A'
max_rv=yaw_rate_run_
D'
q'
exit（）
else:
#发送消息
cmd.linear.x=speed*max_tv;
cmd.angular.z=turn*max_rv;
pub.publish（cmd）
#停止机器人
stop_robot（）;
defstop_robot（）:
cmd.linear.x=0.0
cmd.angular.z=0.0
try:
keyboardLoop（）
exceptrospy.ROSInterruptException:
pass
四、节点关系图
升级会员 