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

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

硬件清单

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

手机端

略懒, 所以这部分没自己写, 直接找商店现成的模拟器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对应Txvoid 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 -> VCC6 -> GND8 -> RX10 -> 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: 16  stamp:     secs: 1452055462    nsecs: 451987028  frame_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: 17  stamp:     secs: 1452055462    nsecs: 468147039  frame_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 pythonfrom datetime import datetimeimport serialimport rospy# Ros Messagesfrom 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_end    joy_msg.buttons = buttons    pub.publish(joy_msg)def cal_xy(rcv):    result = rcv[1:-1]    x = int(result[:-3])    y = int(result[3:])    m = (200 - x) / 100.0    n = (y - 200) / 100.0    return [m, n]while True:    rcv = port.read(8)    # 只收8个字节    if len(rcv) == 8:        result = cal_xy(rcv)        send_cmd(result)

测试

python joy_cmd.pyrostopic 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/bashsource /home/ubuntu/catkin_ws/devel/setup.bashroslaunch turtlebot_bringup minimal.launch

• xbox360_teleop.sh

#!/bin/bashsource /home/ubuntu/catkin_ws/devel/setup.bashroslaunch turtlebot_teleop xbox360_teleop.launch

• launch_ros.sh

#!/bin/bashecho "running turtlebot_bringup"screen -dmS turtlebot_bringup /home/ubuntu/bin/turtlebot_bringup.shsleep 30# 启动很慢, 必须要等turtlebot_bringup启动完毕后才能继续启动下一个, 所以这里设定延时30秒echo "running xbox360_teleop"screen -dmS xbox360_teleop /home/ubuntu/bin/xbox360_teleop.shsleep 15echo "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/ttyAMA0Udevadm info starts with the device specified by the devpath and thenwalks up the chain of parent devices. It prints for every devicefound, all possible attributes in the udev rules key format.A rule to match, can be composed by the attributes of the deviceand 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这个模块的信号很强, 比罗技蓝牙手柄的控制距离远了好多, 稳定性也非常好.

一些问题

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