ROS笔记九（基于Python、Kinetic）：gazebo中通过键盘输入控制turtlebot机器人移动和转向

前言：

Twist消息能描述所有的3D运动，在控制差分驱动的平面运动机器人时，只需要使用其中的两个参数：线性速度（前进/后退）和绕竖直轴的角速度。所以只需要简单地发送线速度（米每秒）和角速度（弧度每秒）命令。这里需要两个程序：一个程序用来监听键盘地敲击事件然后通过ROS消息发布出去，另一个订阅这个ROS消息并输出一个Twist消息，从而实现键盘输入控制机器人地自由移动。

1.键盘驱动：

使用Python的termios和tty库将终端设置成原始模式并捕获键盘敲击事件，然后将事件以std_msgs/string的消息形式发布。

#!/usr/bin/env python

import sys, select, tty, termios
import rospy
from std_msgs.msg import String

if __name__ == '__main__':
  #发布一个‘keys’消息，消息类型String，缓冲为1
  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)

运行效果：

2.运动生成器：

使用w、x、a、d和s是机器人前进、后退、左转、右转和停止。

#!/usr/bin/env python

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] }


def keys_cb(msg, twist_pub):
  #判断输入是否是有效指令
  if len(msg.data) == 0 or not key_mapping.has_key(msg.data[0]):
    return 
  #根据键名取出键值
  vels = key_mapping[msg.data[0]]
  #赋值Twist消息中的线速度和角速度
  t = Twist()
  t.angular.z = vels[0]
  t.linear.x  = vels[1]
  #发出Twist消息
  twist_pub.publish(t)

if __name__ == '__main__':
  #节点初始化
  rospy.init_node('keys_to_twist')
  #声明‘cmd_vel’话题，消息类型为Twist，缓冲为1
  twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
  #订阅‘keys’键盘输入消息，消息类型为String
  rospy.Subscriber('keys', String, keys_cb, twist_pub)
  rospy.spin()

运行效果：

3.代码整合：

将键盘驱动和运动指令生成器整合：

1. 实现速度命令消息流的频率稳定，
2. 添加ROS的参数系统来决定线速度和角速度（满足不同速度需求的机器人）
3. 限制瞬时加速度（瞬间的启动和停止可能会造成打滑，机器人由于驱动电流重复性过载而震颤，甚至机械传动机构断裂）

#!/usr/bin/env python

import rospy
import math
from std_msgs.msg import String
from geometry_msgs.msg import Twist
#使用字典类型存储字母和速度命令的键值映射关系,默认线速度1m/s，默认角速度1rad/s。
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
#接受用户指定的速度范围，默认0.1，低速运行
g_vel_scales = [0.1, 0.1] 
#接受用户指定的线加速度、角加速度（默认值为1）
g_vel_ramps = [1, 1]

#限制速度函数
def ramped_vel(v_prev, v_target, t_prev, t_now, ramp_rate):
  #声明加速度
  step = ramp_rate * (t_now - t_prev).to_sec()
  #判断加速/减速  
  sign = 1.0 if (v_target > v_prev) else -1.0
  #速度变量（目标速度—当前速度）
  error = math.fabs(v_target - v_prev)
  #速度变量小于加速度
  if error < step: 
    #直接返回目标速度
    return v_target
  else:
    #否则增加或减少一个单位的加速度
    return v_prev + sign * step 



#速度参数化
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



#发送Twist速度指令消息
def send_twist():
  global g_last_twist_send_time, g_target_twist, g_last_twist,\
         g_vel_scales, g_vel_ramps, g_twist_pub
  #获取系统当前时间
  t_now = rospy.Time.now()
  #参数化修改速度
  g_last_twist = ramped_twist(g_last_twist, g_target_twist,
                              g_last_twist_send_time, t_now, g_vel_ramps)
  g_last_twist_send_time = t_now
  #发送最终参数化修改后的速度指令消息
  g_twist_pub.publish(g_last_twist)



#接受键盘指令
def keys_cb(msg):
  global g_target_twist, g_last_twist, g_vel_scales
  #判断输入是否是有效指令
  if len(msg.data) == 0 or not key_mapping.has_key(msg.data[0]):
    return 
  #获取keys消息中的速度值
  vels = key_mapping[msg.data[0]]
  #速度参数化，以满足不同速度需求，默认为输入速度的0.1
  g_target_twist.angular.z = vels[0] * g_vel_scales[0]
  g_target_twist.linear.x  = vels[1] * g_vel_scales[1]



#获取用户输入的速度范围参数以及加速度，没有熟度返回默认值
def fetch_param(name, default):
  if rospy.has_param(name):
    return rospy.get_param(name)
  else:
    print "parameter [%s] not defined. Defaulting to %.3f" % (name, default)
    return default

if __name__ == '__main__':
  #节点初始化
  rospy.init_node('keys_to_twist')
  #获取系统当前时间
  g_last_twist_send_time = rospy.Time.now()
  #发布速度指令消息
  g_twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
  #订阅keys消息
  rospy.Subscriber('keys', String, keys_cb)
  #声明Twist消息类型
  g_target_twist = Twist() 
  g_last_twist = Twist()
  #获取用户输入的参数
  g_vel_scales[0] = fetch_param('~angular_scale', 0.1)
  g_vel_scales[1] = fetch_param('~linear_scale', 0.1)
  g_vel_ramps[0] = fetch_param('~angular_accel', 1.0)
  g_vel_ramps[1] = fetch_param('~linear_accel', 1.0)

  rate = rospy.Rate(20)
  while not rospy.is_shutdown():
    send_twist()
    rate.sleep()

4.rqt_plot生成实时速度图：

安装Matplotlib包：

python -m pip install -U matplotlib 

 启动key_publisher.py节点

rosrun teleopbot key_publisher.py 

 打开新的终端，在节点目录启动key_to_twist_with_ramps.py节点,':='语法设置参数的值

./keys_to_twiswith_ramps.py _linear_scale:=0.5 _angular_scale:=0.5 _linear_accel:=1.0 _angular_accel:=1.0 

 可以新开一个终端窗口将keys消息输出到终端

rostopic echo keys

 生成实时速度图：

rqt_plot cmd_vel/linear/x cmd_vel/angular/z

图中，红线表示角速度，蓝线表示线速度，从A点表示一个左转的过程：线速度匀速降低，角速度匀速增加，最大值为0.5，符合输入的参数值，并且两条直线的斜率都为1，同样满足参数的设定。

5.在gazebo中进行仿真，用键盘驱动turtlebot运行：

打开turtlebot_gazebo：

roslaunch turtlebot_gazebo turtlebot_world.launch

运行程序：

./keys_to_twist_with_ramps.py cmd_vel:=cmd_vel_mux/input/teleop

实现在gazebo中通过键盘控制turtlebot机器人自由移动。