ROS键盘遥控机器人，通过参数服务器指定速度

1、引言

在上节的驱动机器人，我们知道是cmd_vel话题发布一串Twist类型消息来控制，我们可以输入如下命令查看这个Twist的详细信息：rosmsg show geometry_msgs/Twist

geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z

由两个向量组成，线速度linear和角速度angular。
然后在命令行，我们对话题cmd_vel发布消息，输入命令如下
rostopic pub /cmd_vel geometry_msgs/Twist -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
这样就可以驱动机器人行驶了，也就说控制差分驱动的平面运动机器人，我们只需要使用上述两个参数：linear就是前进与后退的速度(米/秒)，angular就是绕竖直轴的角速度(弧度/秒)，这个角速度被称为偏航角速度或简单地理解为机器人旋转得有多快。
这节讲解如何遥控机器人，我们通过键盘来控制，所以需要写一个监听键盘敲击的节点，以及在keys话题上发布std_msgs/String的键盘驱动程序。

2、捕获键盘敲击

使用Python的termios和tty库，将终端设置成为原始模式，并捕获键盘的敲击事件，然后将这些事件以std_msgs/String的形式发布出去。
我们接着在上节的工作空间里面来写：cd ~/mywanderbot_ws/src/mywanderbot/src

2.1、key_publisher.py

我们来写一个keys话题，通过这个话题来捕获键盘敲击，然后通过std_msgs.msg.String消息类型来发布:gedit key_publisher.py

#!/usr/bin/env python
import rospy
import sys,select,tty,termios
from std_msgs.msg import String

if __name__=='__main__':
    key_pub = rospy.Publisher('keys',String,queue_size=1)
    rospy.init_node('keyboard_driver')
    rate = rospy.Rate(100)
    old_attr = termios.tcgetattr(sys.stdin)
    #print(old_attr)
    tty.setcbreak(sys.stdin.fileno())
    #print(termios.tcgetattr(sys.stdin))
    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)

加一个可执行权限：chmod u+x key_publisher.py

2.2、获取与设置终端

这个程序使用termios库来捕获键盘敲击，默认地，终端会缓冲文本的一行，直到用户按下回车，才将这一行的文本发送到程序中。那本节，我们是按下任意一个键，我们就要在标准的输入流中获取它，所以需要改变终端的行为。先获取做个备份，然后通过setcbreak更改其为cbreak mode(中断模式),tcgetattr(fd) -> list_of_attrs，获取文件描述符fd的tty属性

模式更改好了之后，我们就可以持续地等待标准输入流，直到有字符出现，虽然我们可以简单地将程序阻塞在标准输入上，但是那样做将导致进程无法触发任何ROS的回调函数，所以这里我们使用select函数，将超时参数设置为0，这样每次调用select函数就不会阻塞，而是立即返回。每次循环中，我们将使用rate.sleep函数消耗掉剩下的时间。
最后在程序退出之前，将终端又设置成标准模式。

termios.tcsetattr(sys.stdin,termios.TCSADRAIN,old_attr)

#tcsetattr解释如下：
tcsetattr(fd, when, attributes) -> None

为文件描述符fd设置tty属性

when：确定何时更改属性
termios.TCSANOW：立即改变
termios.TCSADRAIN：在传送完毕后更改
termios.TCSAFLUSH：传输完毕后改变，队列输出并丢弃所有队列输入

2.3、select

其中select的用法如下

select(rlist, wlist, xlist[, timeout]) -> (rlist, wlist, xlist)

等待，直到一个或多个文件描述符准备好进行某种I/O操作

rlist -- 等待，直到准备好读
wlist -- 等待，直到准备好写
xlist -- 等待，直到有异常

如果只需要一种条件，则为其他列表设置"[]"即可。第四个是可选的超时参数，如果不设置就永不超时。
需要注意的是，在Windows上，只支持套接字，而在Unix上，既支持套接字也支持使用文件描述符。

2.4、捕获测试

我们开三个终端，分别输出如下命令：

roscore

cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py

rostopic echo keys

测试的时候，切换到第二个终端敲击键盘，我们就会在第三个终端打印keys话题信息，进行显示出来。如下图：

3、键盘遥控

我们通过键盘来遥控机器人，看下是如何处理的，通过捕获键盘字母，然后如果是指定的字母，我们就通过Twist消息来输出对应速度。

3.1、keys_to_twist.py

cd ~/mywanderbot_ws/src/mywanderbot/src
gedit keys_to_twist.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

key_mapping={'w':[1,0],'x':[-1,0],'a':[0,1],'d':[0,-1],'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]]
    t=Twist()
    t.linear.x=vels[0]
    t.angular.z=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)
    #print(twist_pub.get_num_connections)
    rospy.Subscriber('keys',String,keys_cb,twist_pub)
    rospy.spin()

这里可以看到，使用key_mapping字典来获取速度，通过订阅keys话题，然后使用它的回调函数keys_cb在字典中查找获取的键名，找到了键名就取出对应的速度，最后通过cmd_vel将键盘获取的速度值发布出去即可。
加上可执行的权限：chmod u+x keys_to_twist.py
分别开启终端来测试下：

roscore

cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py

cd ~/mywanderbot_ws/src/mywanderbot/src
python keys_to_twist.py

我们可以查看cmd_vel的显示情况，在python key_publisher.py这个终端我们敲击键盘，就可以看到速度向量的情况：rostopic echo cmd_vel

linear:
x: 1.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
---
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 1.0
---

当然也可以启动gazebo仿真机器人：roslaunch turtlebot3_gazebo turtlebot3_world.launch
然后我们键盘来控制这个机器人，可以看到机器人在正常运行。

3.2、keys_to_twist_rate.py

我们来看下这个发布消息的平均速率：rostopic hz cmd_vel

subscribed to [/cmd_vel]
no new messages
no new messages
...
average rate: 0.198
min: 0.214s max: 51.695s std dev: 12.05897s window: 24
average rate: 0.204
min: 0.214s max: 51.695s std dev: 11.82527s window: 25
...

我们发现每秒估计一次的平均速率的估计值基本上一直是接近0，也就是仅在我们敲击键盘时，才突然增长一下，然后又变成了0，所以为了让这个节点适用于那些需要稳定的速度命令流的机器人，我们每隔0.1秒(频率为10Hz)就输出一条Twist消息，为了做到这一点，我们在没有收到新的命令时，就重复上一次的命令。
这里我们可以在while循环里面使用sleep(0.1)来实现输出频率的控制，但这样仅仅只是保证输出频率不大于10Hz，而实际的运行频率很可能会由于操作系统的调度和循环本身的耗时出现较大的波动。
因为不同的计算机是有着不同的运行频率和计算性能，为了保证固定不变的输出频率，程序在循环中所需的实际CPU休眠时间也是不能预知的，所以我们使用ROS中的rate结构来实现，它会去持续地估计循环的耗时，获得更一致的结果。
我们将其改进如下： cd ~/mywanderbot_ws/src/mywanderbot/src
gedit keys_to_twist_rate.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

key_mapping={'w':[1,0],'x':[-1,0],'a':[0,1],'d':[0,-1],'s':[0,0]}
g_last_twist=None

def keys_cb(msg,twist_pub):
    global g_last_twist
    if len(msg.data)==0 or not key_mapping.has_key(msg.data[0]):
        return 
    vels=key_mapping[msg.data[0]]
    g_last_twist.linear.x=vels[0]
    g_last_twist.angular.z=vels[1]
    twist_pub.publish(g_last_twist)

if __name__=='__main__':
    rospy.init_node('keys_to_twist_rate')
    twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
    rospy.Subscriber('keys',String,keys_cb,twist_pub)
    rate=rospy.Rate(10)
    g_last_twist=Twist()
    while not rospy.is_shutdown():
        twist_pub.publish(g_last_twist)
        rate.sleep()

加个可执行权限：chmod u+x keys_to_twist_rate.py

分别开启终端并执行下面命令：

roscore

cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py

cd ~/mywanderbot_ws/src/mywanderbot/src
python keys_to_twist_rate.py

最后我们新开一个终端，再来看下输出速率：rostopic hz cmd_vel

subscribed to [/cmd_vel]
average rate: 10.002
   min: 0.099s max: 0.101s std dev: 0.00045s window: 10
average rate: 9.998
   min: 0.099s max: 0.101s std dev: 0.00050s window: 20
average rate: 9.996
   min: 0.099s max: 0.101s std dev: 0.00049s window: 30
average rate: 10.000
   min: 0.098s max: 0.103s std dev: 0.00081s window: 40
average rate: 10.000
   min: 0.098s max: 0.103s std dev: 0.00078s window: 50
average rate: 10.000
   min: 0.098s max: 0.103s std dev: 0.00079s window: 60
...

可以看到结果，基本是稳定的10Hz的输出速率。

3.3、rqt_graph

我们可以查看下目前有哪些相关节点，以及它们之间的关系，可以输入：rostopic info cmd_vel

Type: geometry_msgs/Twist

Publishers:
* /keys_to_twist (http://YAB:41117/)
* /keys_to_twist_rate (http://YAB:40317/)

Subscribers:
* /rostopic_5913_1696473604053 (http://YAB:46119/)
* /rostopic_5956_1696473632865 (http://YAB:45937/)
* /rostopic_6042_1696476700529 (http://YAB:44365/)
* /rostopic_6083_1696476719297 (http://YAB:33923/)
* /rostopic_6475_1696476841619 (http://YAB:45707/)

可以看到有两个发布者，下面就是很多的订阅者。

类型是geometry_msgs/Twist，我们可以详细查看其结构：rosmsg show geometry_msgs/Twist

geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z

可以看到成员类型是geometry_msgs/Vector3，有三个域分别是x、y、z

我们还可以对其可视化，输入：rqt_graph

3.4、rqt_plot

从上面我们知道了话题名称和消息中域的名字，我们可以选择感兴趣的域，将这些数据流绘制出来，那么这节主要就是线速度X跟角速度Z，所以我们对这两个域进行绘制
rqt_plot cmd_vel/linear/x cmd_vel/angular/z
当我们按下键的时候，速度命令就会改变，我们来看下绘制的情况，暂停截图如下：

4、参数服务器

上面的速度绘制图，我们观察发现，速度一直是0、1、-1三种取值，在ROS中使用的是SI(国际)单位制，也就是机器人以每秒一米的速度向前向后，以每秒一弧度的角速度左右旋转。
然而，在不同场景，机器人的运行速度是完全不一样的，有的地方1m/s速度太慢了，有的地方却可能太快了，所以我们需要找到一种把程序参数化的方法，这样它才能应用到不同的机器人。

cd ~/mywanderbot_ws/src/mywanderbot/src
gedit keys_to_twist_parameterized.py

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

key_mapping={'w':[1,0],'x':[-1,0],'a':[0,1],'d':[0,-1],'s':[0,0]}
g_last_twist=None
g_vel_scales=[0.1,0.1]

def keys_cb(msg,twist_pub):
    global g_last_twist,g_vel_scales
    if len(msg.data)==0 or not key_mapping.has_key(msg.data[0]):
        return
    vels=key_mapping[msg.data[0]]
    g_last_twist.linear.x=vels[0]*g_vel_scales[0]
    g_last_twist.angular.z=vels[1]*g_vel_scales[1]
    twist_pub.publish(g_last_twist)

if __name__=='__main__':
    rospy.init_node('keys_to_twist_parameterized')
    twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)
    rospy.Subscriber('keys',String,keys_cb,twist_pub)
    g_last_twist=Twist()
    if rospy.has_param('~linear_scale'):
        g_vel_scales[0]=rospy.get_param('~linear_scale')
    else:
        rospy.logwarn('linear_scale not provided,using %.1f'%g_vel_scales[0])
    if rospy.has_param('~angular_scale'):
        g_vel_scales[1]=rospy.get_param('~angular_scale')
    else:
        rospy.logwarn('angular_scale not provided,using %.1f'%g_vel_scales[1])

    rate=rospy.Rate(10)
    while not rospy.is_shutdown():
        twist_pub.publish(g_last_twist)
        rate.sleep()

加个可执行权限：chmod u+x keys_to_twist_parameterized.py

分别开启终端，输入如下命令：

roscore

cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py

cd ~/mywanderbot_ws/src/mywanderbot/src
./keys_to_twist_parameterized.py

这里如果没有指定参数就会出现警告，输入不带参数： ./keys_to_twist_parameterized.py

[WARN] [1696558099.873788]: linear_scale not provided,using 0.1
[WARN] [1696558099.875360]: angular_scale not provided,using 0.1

日志保存在cd ~/.ros/log这里里面的对应文件：cat keys_to_twist_parameterized.log

当然日志太多，用不着的话可以全部清理：rospy purge

正常带参数的输入：./keys_to_twist_parameterized.py _linear_scale:=0.8 _angular_scale:=0.7
这里的参数赋值，需要注意的是使用的是海象运算符(:=)

最后再开一个终端我们来测试下新的速度是否有变化：rostopic echo cmd_vel
然后我们在运行key_publisher.py的这个终端，按键w，将出现如下速度：

linear:
x: 0.8
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0

按键a，将出现如下速度：

linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.7

可以看到，参数的指定也更改了对应的速度值。

5、加速度

我们知道在现实世界里面，任何物体都是有质量的，所以机器人不能瞬间启动与停止，而是需要一个加速与减速的过程，而且当速度立马切换到非常快的时候，开车的朋友都清楚，容易出现打滑等情况，所以我们也不能给机器人发送不合理的速度命令，为了避免这些问题，我们把速度命令设置成一个斜坡上升或下降的过程。

cd ~/mywanderbot_ws/src/mywanderbot/src
gedit keys_to_twist_ramps.py

加个可执行权限：chmod u+x keys_to_twist_ramps.py

我们可以先来测试下，再次之前需要启动roscore，然后输入：./keys_to_twist_ramps.py

parameter [~linear_scale] not found,using 0.100
parameter [~angular_scale] not found,using 0.100
parameter [~linear_accel] not found,using 1.000
parameter [~angular_accel] not found,using 1.000

这段代码，就是在前面的基础上，做一个速度阶跃的增加与减少，这样即使我们发给机器人是瞬时变化的速度或者是阶跃式的命令，都会被减缓成斜坡状。主要就是ramped_vel这个函数，每次被调用，都会向目标速度前进一步，如果距离目标速度小于一个step步长的话，就直接赋值目标速度。然后通过ramped_twist函数计算得到twist，再通过send_twist进行发布即可。后面的回调函数以及获取参数的方法跟前面差不多，这里就不赘述了。
整体来测试下，分别开启终端，输入如下命令

roscore

cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py

cd ~/mywanderbot_ws/src/mywanderbot/src
./keys_to_twist_ramps.py _linear_scale:=0.5 _angular_scale:=1.0 _linear_accel:=1.0 _angular_accel:=1.0

跟前面一样，我们使用rqt_plot程序来生成一个实时的速度图
rqt_plot cmd_vel/linear/x cmd_vel/angular/z

在key_publisher.py这个终端进行按键测试，我们就能够实时看到绘制的速度图了。跟前面的速度绘制图进行比较，我们也可以发现，速度在上升与下降的过程都消耗了一定的时间，加速度有限，所以这些命令在现实中是可以实现的。
也可以直接观察仿真机器人的行驶情况，打开命令
roslaunch turtlebot3_gazebo turtlebot3_world.launch

6、launch文件

对于上面的节点，我们每次要启动很多终端，比较繁琐，现在我们来回顾下以前学习过的launch文件，可以批量启动节点，有兴趣的可以查阅：ROS机器人操作系统Catkin的编译与常用命令的使用介绍里面有对launch文件的更具体介绍

操作如下：

cd ~/mywanderbot_ws/src/mywanderbot
mkdir launch
cd launch
gedit test.launch

<launch>
<node name="key_publisher" pkg="mywanderbot" type="key_publisher.py" respawn="false" output="screen" />
<node name="keys_to_twist_ramps" pkg="mywanderbot" type="keys_to_twist_ramps.py" respawn="false" output="screen">
  <param name="linear_scale" type="double" value="0.5" />
  <param name="angular_scale" type="double" value="1.0" />
  <param name="linear_accel" type="double" value="1.0" />
  <param name="angular_accel" type="double" value="1.0" />
</node>
</launch>

加个可执行权限：chmod u+x test.launch

执行launch文件：roslaunch mywanderbot test.launch
这样就启动了定义的两个节点，以及将定义的参数也会加载到参数服务器里面
我们可以查看下实时速度：rqt_plot cmd_vel/linear/x cmd_vel/angular/z
如下图：