前文中记录了随机移动机器人的开发过程，本文内容为Turtlebot3 Gazebo仿真环境下Teleop-bot 键盘操控移动机器人，主要包含以下几个部分：
1、键盘驱动（按键驱动发布keys话题）
2、运动生成器（订阅keys话题发布cmd_vel话题）
3、速度斜坡曲线
4、参数服务器
6、rviz 机器人、传感器和算法 3D可视化系统使用
1、键盘驱动（按键驱动发布keys话题）
代码及注释如下：
#!/usr/bin/env python
#-*- coding:utf-8 -*-
import sys
import select
import tty
import termios
import rospy
from std_msgs.msg import String
key_pub=rospy.Publisher('keys',String,queue_size=1)
rospy.init_node('keyboard_driver')
rate=rospy.Rate(100)
# 保存原来属性
old_attr=termios.tcgetattr(sys.stdin)
# 设置为单字符响应模式
tty.setcbreak(sys.stdin.fileno())
print "Publishing keystrokes. Press Ctrl+C to exit..."
while not rospy.is_shutdown():
if select.select([sys.stdin],[],[],0)[0]==[sys.stdin]:
# 发布按键
key_pub.publish(sys.stdin.read(1))
rate.sleep()
#恢复属性
termios.tcsetattr(sys.stdin,termios.TCSADRAIN,old_attr)可以通过rostopic echo命令查看keys 话题发布的消息：
wsc@wsc-pc:~/wanderbot_ws$ rostopic echo keys
data: "a"
---
data: "a"
---
data: "a"
---
data: "z"
---
data: "s"2、运动生成器（订阅keys话题发布cmd_vel话题）
运动生成器代码及注释如下：
#!/usr/bin/env python
#-*- coding:utf-8 -*-
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
# 定义按键定义
key_mapping={
'w':[0,1],
'x':[0,-1],
'a':[-1,0],
'd':[1,0],
's':[0,0]
}
# 接收keys话题回调函数
def keys_callback(msg,twist_pub):
if len(msg.data)==0 or (not key_mapping.has_key(msg.data[0])):
return
vels=key_mapping[msg.data[0]]
t=Twist()
t.angular.z=vels[0]
t.linear.x=vels[1]
twist_pub.publish(t)
if __name__ == '__main__':
rospy.init_node('keys_to_twist')
twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
keys_sub=rospy.Subscriber('keys',String,keys_callback,twist_pub)
rospy.spin()运行rqt_plot 命令显示发布的话题数据：
3、参数服务器
上面我们按下键盘给出了固定的速度大小，我们可以使用ROS的参数系统来决定线速度和角速度，另外，上面的程序仅在按键时发布一条消息，我们可以利用rospy.Rate(10)改进代码使得程序以10HZ的频率稳定地发布话题。改进后的代码及注释如下：
#!/usr/bin/env python
#-*- coding:utf-8 -*-
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
# 定义按键定义
key_mapping={
'w':[0,1],
'x':[0,-1],
'a':[-1,0],
'd':[1,0],
's':[0,0]
}
g_last_twist=None
# 速度因子参数
g_vel_scals=[0.1,0.1]
# 接收keys话题回调函数
def keys_callback(msg,twist_pub):
global g_last_twist,g_vel_scals
if len(msg.data)==0 or (not key_mapping.has_key(msg.data[0])):
return
vels=key_mapping[msg.data[0]]
g_last_twist=Twist()
g_last_twist.angular.z=vels[0]*g_vel_scals[0]
g_last_twist.linear.x=vels[1]*g_vel_scals[0]
twist_pub.publish(g_last_twist)
if __name__ == '__main__':
rospy.init_node('keys_to_twist')
twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
keys_sub=rospy.Subscriber('keys',String,keys_callback,twist_pub)
g_last_twist=Twist()
# 判断是否有linear_scale参数
if rospy.has_param('~linear_scale'):
g_vel_scals[1]=rospy.get_param('~linear_scale')
else:
rospy.logwarn("linear_scale not provided,using %.1f"%g_vel_scals[1])
# 判断是否有angular_scale参数
if rospy.has_param('~angular_scale'):
g_vel_scals[0]=rospy.get_param('~angular_scale')
else:
rospy.logwarn("angular_scale not provided,using %.1f"%g_vel_scals[0])
rate=rospy.Rate(10)
while not rospy.is_shutdown():
twist_pub.publish(g_last_twist)
rate.sleep()设置参数值方法 ：rosrun teleop_bot keys_to_twist_with_ramps.py _linear_scale:=0.5 _angular_scale:=0.5
4、速度斜坡曲线
从第2步中发布的cmd_vel 话题数据图形显示可以看出，速度从1到-1是突变的，但是实际的物体不能瞬时启动、停止，机器人电机瞬间切到一个相差较大的速度时，可能会发生严重的后果。底层的机器人驱动器固件可能会对控制加速度进行平滑，但是最科学的办法还是在向机器人发布cmd_vel时就考虑到这一点，下面给出了改进的代码，限制了瞬时加速度。
#!/usr/bin/env python
#-*- coding:utf-8 -*-
import rospy
import math
from std_msgs.msg import String
from geometry_msgs.msg import Twist
# 定义按键定义
key_mapping={
'w':[0,1],
'x':[0,-1],
'a':[-1,0],
'd':[1,0],
's':[0,0]
}
g_twist_pub=None
g_target_twist=None
g_last_twist=None
g_last_send_time=None
# 速度因子参数
g_vel_scals=[0.1,0.1]
g_vel_ramps=[1.0,1.0]
def fetch_params(name,default):
# 判断是否有name参数
if rospy.has_param(name):
return rospy.get_param(name)
else:
print "%s not provided,using %.1f"%(name,default)
return default
def send_twist():
global g_last_twist,g_target_twist,g_last_send_time,g_vel_ramps,g_twist_pub
t_now=rospy.Time.now()
g_last_twist=ramped_twist(g_last_twist,g_target_twist,g_last_send_time,t_now,g_vel_ramps)
g_twist_pub.publish(g_last_twist)
g_last_send_time=t_now
def ramped_vel(v_prev,v_target,t_prev,t_now,ramp_rate):
step=(t_now-t_prev).to_sec()*ramp_rate
if v_target>v_prev:
sign=1
else:
sign=-1
if (math.fabs(v_target-v_prev))>step:
return v_prev+step*sign
else:
return v_target
def ramped_twist(prev,target,t_prev,t_now,ramps):
tw=Twist()
tw.angular.z=ramped_vel(prev.angular.z,target.angular.z,t_prev,t_now,ramps[0])
tw.linear.x=ramped_vel(prev.linear.x,target.linear.x,t_prev,t_now,ramps[1])
return tw
# 接收keys话题回调函数
def keys_callback(msg):
global g_target_twist,g_vel_scals
if len(msg.data)==0 or (not key_mapping.has_key(msg.data[0])):
return
vels=key_mapping[msg.data[0]]
g_target_twist=Twist()
g_target_twist.angular.z=vels[0]*g_vel_scals[0]
g_target_twist.linear.x=vels[1]*g_vel_scals[0]
if __name__ == '__main__':
rospy.init_node('keys_to_twist')
g_last_send_time=rospy.Time.now()
g_twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
keys_sub=rospy.Subscriber('keys',String,keys_callback)
g_last_twist=Twist()
g_target_twist=Twist()
g_vel_scals[1]=fetch_params('~linear_scale',g_vel_scals[1])
g_vel_scals[0]=fetch_params('~angular_scale',g_vel_scals[0])
g_vel_ramps[1]=fetch_params('~linear_accl',g_vel_ramps[1])
g_vel_ramps[0]=fetch_params('~angular_accl',g_vel_ramps[0])
rate=rospy.Rate(10)
while not rospy.is_shutdown():
send_twist()
rate.sleep()结果如下图，速度不再突变：
6、rviz 机器人、传感器和算法 3D可视化系统使用
rviz可以通过RoboWare Studio 菜单打开，也可以通过终端命令打开。
（1）选择参考坐标系，过程如下：
（2）添加机器人模型
（3）添加传感器
（4）转换视角观察
（5）在rviz中观察键盘驱动Turtlebot3运动
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
