使用键盘发布手柄控制信息

最近在看CMU的ShuoYang开源的自己写的MIT cheetah3的控制代码，在经过了漫长的docker环境安装后，发现因为自己没有xbox手柄，因此无法直接控制gazebo中的机器狗，因此准备使用键盘来模拟手柄发布信息。
大家对机器狗感兴趣的也可以去运行一下跑一跑看一看，因为使用docker部署环境，所以可以省掉复杂的环境配置过程（只是对网络要求可能有点高）。这里也给出大佬的知乎文章，其实是github的简中版。
知乎：https://zhuanlan.zhihu.com/p/504495840?utm_source=wechat_session&utm_medium=social&utm_oi=28147256918016&s_r=0
gihub：https://github.com/ShuoYangRobotics/A1-QP-MPC-Controller

接下来就介绍一下我如何使用键盘映射到手柄的信息。

问题解决思路

通过查看源码得知，控制gazebo中的机器狗是通过订阅一个/joy的主题发布的sensor_msgs/Joy信息来进行的，因此就想到能不能直接获取键盘的按键信息然后映射到手柄上对应的键位，最后将其打包成sensor_msgs/Joy的样式再通过将信息发布至/joytopic解决这个问题。

解决方法

一番搜索后找到了stanfordroboticsclub所写的一个键盘控制器，同样实现的是使用键盘发布手柄信息。这个脚本基于pygame来捕获键盘按键信息，然后使用一个简单的迭代来计算模拟摇杆键程信息。然后我就在这个脚本的基础上进行了一定的修改，将获得的按键信息打包成sensor_msgs/Joy的信息，然后使用rospy将消息发布到/joy。
至于想要发布什么样的信息可以自己进行设定，因为通过查看sensor_msgs/Joy发现其结构除了头部信息以外只是两个数组：float64 axes[]和int32 buttons[]。至于里面的信息顺序该如何存储，并没有规定，因此完全可以自己定义数据的顺序。
要注意的是上面所介绍的脚本所写的控制器是基于PS4手柄的，可以从它的命名看出，分别使用了x、square、circle、triangle四个标志表示四个按钮，对应了xbox中的A、X、B、Y。
这里给出大体的打包成Joy信息的流程，不在给出整个实现代码。

rospy.init_node("joy_node")
pub = rospy.Publisher("/joy",Joy,queue_size=1)
...
while True:out_msg = Joy()msg = {"ly": 0,"lx": 0,"rx": 0,"ry": 0,"L2": 0,"R2": 0,"R1": 0,"L1": 0,"dpady": 0,"dpadx": 0,"x": 0,"square": 0,"circle": 0,"triangle": 0,"message_rate": MESSAGE_RATE,}out_msg.axes = [msg["lx"],msg["ly"],msg["rx"],msg["L2"],msg["R2"],msg["ry"],msg["dpadx"],msg["dpady"]]out_msg.buttons = [msg["x"],msg["circle"],msg["square"],msg["triangle"],msg["L1"],msg["R1"],0,0,0,0,0]...out_msg.header.stamp = rospy.Time.now()pub.publish(out_msg)

使用

roscore
python keyboard_joystick.py

使用上述脚本时记得退出conda环境，在自己的环境中安装pygame

可能存在的问题

如果感觉发送频率不匹配，记得将里面的print()函数注释掉，避免不必要的运行开支。如果注释掉还是感觉卡，就降低一下每秒发送信息的频率，可以通过调整脚本中的MESSAGE_RATE实现。
如果发现时间戳信息均为0，试一试把所有的ros相关的东西全部关闭，然后重启roscore再启动脚本试一下。

参考资料：

https://github.com/stanfordroboticsclub/PupperKeyboardController
https://zhuanlan.zhihu.com/p/504495840?utm_source=wechat_session&utm_medium=social&utm_oi=28147256918016&s_r=0
https://github.com/ShuoYangRobotics/A1-QP-MPC-Controller
http://wiki.ros.org/joy