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 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()

结果如下图，速度不再突变：