ROS开发笔记（5）——基于 python 开发 Turtlebot3 Gazebo仿真环境下键盘操控移动机器人（Teleop-bot ）

前文中记录了随机移动机器人的开发过程，本文内容为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 Stringkey_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])):returnvels=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_scalsif len(msg.data)==0 or (not key_mapping.has_key(msg.data[0])):returnvels=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=Noneg_target_twist=None
g_last_twist=Noneg_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 defaultdef send_twist():global g_last_twist,g_target_twist,g_last_send_time,g_vel_ramps,g_twist_pubt_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_nowdef ramped_vel(v_prev,v_target,t_prev,t_now,ramp_rate):step=(t_now-t_prev).to_sec()*ramp_rateif v_target>v_prev:sign=1else:sign=-1if (math.fabs(v_target-v_prev))>step:return v_prev+step*signelse:return v_targetdef 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_scalsif len(msg.data)==0 or (not key_mapping.has_key(msg.data[0])):returnvels=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运动