手机模拟蓝牙手柄控制kobuki底座

最近在做ros相关的东西, 硬件平台kobuki, 成功实现手机模拟蓝牙手柄控制kobuki底座, 在这里记录一下折腾过程

硬件清单

HC-05 主从模块各一(可以任意转换M/S模式)
树莓派2 x1
Arduino UNO x1
USB蓝牙适配器 x1
面包板 x1
安卓手机 x1
杜邦线若干

手机端

略懒, 所以这部分没自己写, 直接找商店现成的模拟器app

Play商店搜索适合的APP

选定 Joy BT Commander 链接自备爬墙梯

蓝牙模块

Serial Port Bluetooth Module (Master/Slave) : HC-05

模块参数设定

参考上述wiki链接, 我用的串口调试软件是: Serial Communicator用到的AT命令主要有:

查看/更改匹配PIN码

AT+PSWD? # 查
AT+PSWD=1234 # 改为1234

更改主从模式

AT+ROLE? # 查
AT+ROLE=0 # 改 0- Slave 1-Master 2-Slave-Loop

更改设备名

AT+NAME? # 查
AT+NAME=MY_BLUE # 改

更改UART(这里改115200后面有用)

AT+UART? # 查
AT+UART=115200,1,0 # 改 change default 9600 to 115200, 1 stop bit, 0 parity

注意: 有的板子可能没有key引脚, 而是state, en引脚附近有一个硬按键, 这种板子进入AT模式需要在上电之前按住这个按键不放, 然后上电, 待LED灯闪烁间隔大约两秒一次时, 说明进入AT模式成功, 此模式下默认的UART参数是38400,1,0

与Arduino通信

测试代码:

#include <SoftwareSerial.h>
SoftwareSerial Conn(10, 11); //RX, TX 接线Rx对应Tx
void setup() {// put your setup code here, to run once:Conn.begin(115200);
}void loop() {// put your main code here, to run repeatedly:Conn.println("Test Transmission String.");delay(600);
}

此时打开调试软件接通端口会收到循环的Test Transmission String.字符串

与树莓派直连通信

引脚定义见链接: Raspberry Pi Model B+ and Raspberry Pi 2 GPIO layout

接线

4 -> VCC
6 -> GND
8 -> RX
10 -> TX

树莓派测试程序

import serialport = serial.Serial("/dev/ttyAMA0", baudrate=115200, timeout=0.5)while True:port.write("\r\nSay something:")rcv = port.read(100)port.write("\r\nYou sent:" + repr(rcv))

输出如图:

通讯协议

分析模拟APP

Joy BT Commander本身很强大, 见原帖, 我只用到其中的摇杆数据, 软件发送的数据格式为标准ASCII码, 具体是形如x02200200x03共八个字节, 第一个字节是开始符, 最后一个为终止符, 中间6位是实际坐标, 前三位表示横坐标, 后三位表示纵坐标, 默认坐标原点为(200, 200)最左为100最上为300

.	.	.
100,300	200,300	300,300
100,200	200,200	300,200
100,100	200,100	300,100

分析手柄

插上手柄, 监听/joy话题消息内容 http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick

消息内容如下:

---
header: seq: 16stamp: secs: 1452055462nsecs: 451987028frame_id: ''
axes: [0.10000000149011612, -0.009999999776482582, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
buttons: [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
---
header: seq: 17stamp: secs: 1452055462nsecs: 468147039frame_id: ''
axes: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
buttons: [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
---

其中有意义的是axes的前两位, 分别表示横纵坐标; buttons的第五位必须为1. 剩下的seq为系统自动生成, 表示消息序号; stamp为时间戳, 这个作用不大, 不处理也可以.

注意: 手柄实际坐标取值范围是[-1, 1], 原点坐标为(0, 0), 横坐标向左为正, 纵坐标正常, 需要转换

转换消息

明显手机发出的数据和/joy话题需要收到的标准手柄数据不一样, 所以这里需要写一个程序负责把蓝牙串口收到的x02200200x03数据包装成类似上面获取到的手柄真实数据格式, 然后发布给/joy这个topic, 以达到冒充手柄的目的

假定手机app发送给ROS系统的坐标为(x, y), 那么对应的手柄坐标(m, n)就应该是:

m = (200 - x) / 100 # 横坐标是反的
n = (y -200) / 100 # 纵坐标正常

ROS系统

模拟手柄数据

上面说到需要有一个程序收取蓝牙串口数据并伪装成真手柄数据发布到/joy这个topic, 需要写一个脚本来处理, 内容如下(joy_cmd.py):

#!/usr/bin/env python
from datetime import datetimeimport serialimport rospy# Ros Messages
from sensor_msgs.msg import Joyport = serial.Serial("/dev/ttyAMA0", baudrate=115200, timeout=0.5)
buttons = [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
axes_end = [0, 0, 0, 0, 0, 0]def send_cmd(m_n):pub = rospy.Publisher("/joy", Joy, queue_size=10)rospy.init_node('joy_cmd')joy_msg = Joy()joy_msg.header.stamp = rospy.Time.now() # 这里stamp不做处理也可以joy_msg.axes = m_n + axes_endjoy_msg.buttons = buttonspub.publish(joy_msg)def cal_xy(rcv):result = rcv[1:-1]x = int(result[:-3])y = int(result[3:])m = (200 - x) / 100.0n = (y - 200) / 100.0return [m, n]while True:rcv = port.read(8)# 只收8个字节if len(rcv) == 8:result = cal_xy(rcv)send_cmd(result)

测试

python joy_cmd.py
rostopic echo /joy

连接蓝牙, 操作手机APP摇杆, 观察终端输出数据, 如图, 成功!

依次启动相关launch来测试效果吧:

roslaunch turtlebot_bringup minimal.launch # 驱动kobuki最小系统
roslaunch turtlebot_teleop xbox360_teleop.launch # 启动手柄驱动
python joy_cmd.py

自启动设定

至此, 已经可以用手机模拟器控制kobuki了, 但比较麻烦的问题是ROS的这些launch都只能维持在前台状态, 需要找一种略和谐的方式来把launch放在后台运行, 在需要的时候随时可切换查看log日志, 当然要是可以随系统自动启动那就更完美了. 尝试了以下几种方式来自启动这些launch

supervisor

环境变量env不好处理, 总是失败, 暂时放弃…

ros_upstart

ros官方推荐的处理方式, 然而打开他的帮助文档一看, 内容好少, 简单试了下, 跟supervisor一样的问题, 环境变量配置过程中source命令总出错, 遂放弃…

screen

这可能是我目前能想到最容易实现的方法, 把launch封装成shell脚本, 然后在/etc/rc.local中依次调用, 具体内容如下
- turtlebot_bringup.sh

#!/bin/bash
source /home/ubuntu/catkin_ws/devel/setup.bash
roslaunch turtlebot_bringup minimal.launch

xbox360_teleop.sh

#!/bin/bash
source /home/ubuntu/catkin_ws/devel/setup.bash
roslaunch turtlebot_teleop xbox360_teleop.launch

launch_ros.sh

#!/bin/bash
echo "running turtlebot_bringup"
screen -dmS turtlebot_bringup /home/ubuntu/bin/turtlebot_bringup.sh
sleep 30
# 启动很慢, 必须要等turtlebot_bringup启动完毕后才能继续启动下一个, 所以这里设定延时30秒
echo "running xbox360_teleop"
screen -dmS xbox360_teleop /home/ubuntu/bin/xbox360_teleop.sh
sleep 15
echo "running joy_cmd"
screen -dmS joy_cmd python joy_cmd.py

/etc/rc.local

# 添加自启动, 以ubuntu用户执行
su ubuntu -c "/home/ubuntu/bin/launch_ros.sh"

效果测试

还遇到一个问题是: 蓝牙模块通过GPIO引脚接入树莓派后, 默认指定的设备文件
/dev/ttyAMA0权限不够, 非root用户无法读取, 所以需要给单独写个udev规则文件

# 获取模块硬件信息
ududevadm info -a -n /dev/ttyAMA0
Udevadm info starts with the device specified by the devpath and then
walks up the chain of parent devices. It prints for every device
found, all possible attributes in the udev rules key format.
A rule to match, can be composed by the attributes of the device
and the attributes from one single parent device.looking at device '/devices/dev:f1/tty/ttyAMA0':KERNEL=="ttyAMA0"SUBSYSTEM=="tty"DRIVER==""looking at parent device '/devices/dev:f1':KERNELS=="dev:f1"SUBSYSTEMS=="amba"DRIVERS=="uart-pl011"ATTRS{id}=="00341011"ATTRS{irq0}=="83"

根据获取到的硬件信息在这里/etc/udev/rules.d新建一个设备规则60-hc05.rules

SUBSYSTEM=="tty", ATTRS{id}=="00341011", MODE:="0666", GROUP:="dialout"

权限给个RW就行, 然后重启系统查看/dev/ttyAMA0权限, 如下:
crw-rw-rw- 1 root dialout 204, 64 Jan 6 11:55 /dev/ttyAMA0

终于, 一切正常, 系统自动启动, 不依赖host机器通过ssh预先启动launch, 可独立被手机控制行走.

测试过程中发现HC-05这个模块的信号很强, 比罗技蓝牙手柄的控制距离远了好多, 稳定性也非常好.

一些问题

控制信息只处理了摇杆坐标信息, 有待添加按键, 自定义数据支持
screen方法还是不太稳定, minimal.launch进程有概率挂掉
如发生程序异常退出, 系统无法自动恢复, 需要重启或者接入host主机才能处理, 健壮性不足, 后续还是应该考虑supervisor的监控方案,
目前只用蓝牙2.0模块做了测试, 控制距离大概50米左右, 不知道换4.0/1模块会有怎样的提升