Add script to bind rfcomm device to remote Bluetooth device

jsk_pr2_robot/jsk_pr2_startup/README.md

@@ -57,3 +57,43 @@ $ sudo setcap cap_fowner+ep /usr/bin/mongod
 - hark jsk installation: https://github.com/jsk-ros-pkg/jsk_3rdparty/blob/master/hark_jsk_plugins/INSTALL
 - Microcone: http://www.hark.jp/wiki.cgi?page=SupportedHardware#p10
 
+
+### Bind rfcomm device
+
+By binding rfcomm device, we can connect bluetooth device via device file (e.g. `/dev/rfcomm1`). For example, rosserial with [this PR](https://github.com/ros-drivers/rosserial/pull/569) can be used over bluetooth connection.
+
+For detail, please see https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_robot_common/jsk_robot_startup/README.md#launchrfcomm_bindlaunch
+
+#### usage
+
+Save the bluetooth device MAC address to file like `/var/lib/robot/rfcomm_devices.yaml` in PR2.
+
+```
+- name: device1
+  address: XX:XX:XX:XX:XX:XX
+- name: device2
+  address: YY:YY:YY:YY:YY:YY
+```
+
+Then, bind rfcomm devices.
+
+```
+# login as root user in pr2
+ssh pr2
+su
+# Assume the bluetooth dongle is plugged into c2
+roslaunch jsk_pr2_startup pr2_rfcomm_bind.launch machine:=c2
+```
+
+To check how many devices are bound to rfcomm, use rfcomm command.
+```
+ssh pr2
+ssh c2
+rfcomm
+```
+
+#### management
+
+Currently in PR2, `pr2_rfcomm_bind.launch` is started automatically by upstart.
+
+The upstart config file is in `/etc/upstart/jsk-rfcomm-bind.conf` in PR2.

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_sensors/pr2_rfcomm_bind.launch

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="machine" default="localhost"/>
+  <arg name="rfcomm_devices" default="/var/lib/robot/rfcomm_devices.yaml" />
+
+  <include file="$(find pr2_machine)/$(env ROBOT).machine" />
+
+  <include file="$(find jsk_robot_startup)/launch/rfcomm_bind.launch">
+    <arg name="machine" value="$(arg machine)" />
+    <arg name="rfcomm_devices" value="$(arg rfcomm_devices)" />
+  </include>
+</launch>

jsk_robot_common/jsk_robot_startup/README.md

@@ -144,3 +144,29 @@ This node publish the luminance calculated from input image and room light statu
 This launch file provides a set of nodes for safe teleoperation common to mobile robots. Robot-specific nodes such as `/joy`, `/teleop` or `/cable_warning` must be included in the teleop launch file for each robot, such as [safe_teleop.xml for PR2](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml) or [safe_teleop.xml for fetch](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml).
 
 ![JSK teleop_base system](images/jsk_safe_teleop_system.png)
+
+## launch/rfcomm_bind.launch
+
+This script binds rfcomm device to remote bluetooth device. By binding rfcomm device, we can connect bluetooth device via device file (e.g. `/dev/rfcomm1`). For example, rosserial with [this PR](https://github.com/ros-drivers/rosserial/pull/569) can be used over bluetooth connection.
+
+### Usage
+
+Save the bluetooth device MAC address to file like `/var/lib/robot/rfcomm_devices.yaml`.
+
+```
+- name: device1
+  address: XX:XX:XX:XX:XX:XX
+- name: device2
+  address: YY:YY:YY:YY:YY:YY
+```
+
+Then, bind rfcomm devices.
+
+```
+roslaunch jsk_robot_startup rfcomm_bind.launch
+```
+
+To check how many devices are bound to rfcomm, use rfcomm command.
+```
+rfcomm
+```

jsk_robot_common/jsk_robot_startup/launch/rfcomm_bind.launch

@@ -0,0 +1,13 @@
+<launch>
+  <arg name="machine" default="localhost"/>
+  <arg name="rfcomm_devices" default="/var/lib/robot/rfcomm_devices.yaml" />
+
+  <machine name="localhost" address="localhost"/>
+
+  <node name="bind_rfcomm" pkg="jsk_robot_startup" type="rfcomm_bind.py"
+        output="screen" machine="$(arg machine)">
+    <rosparam subst_value="true">
+      rfcomm_devices: $(arg rfcomm_devices)
+    </rosparam>
+  </node>
+</launch>

jsk_robot_common/jsk_robot_startup/scripts/rfcomm_bind.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+
+import os
+import rospy
+import subprocess
+from subprocess import PIPE
+import yaml
+
+
+class RfcommBind(object):
+    """
+    This node binds the RFCOMM device to a remote Bluetooth device.
+    MAC address of bound devices must be stored in '~rfcomm_devices' file.
+
+    '~rfcomm_devices' yaml file is like the following.
+    name is device name for human to distinguish.
+    address is device's bluetooth MAC address
+    - name: device1
+      address: XX:XX:XX:XX:XX:XX
+    - name: device2
+      address: YY:YY:YY:YY:YY:YY
+
+    Note that this node is called with sudo.
+
+    For detail of rfcomm, run this command:
+    $ man rfcomm
+    """
+
+    def __init__(self):
+        self.rfcomm_devices = {}
+        yaml_path = rospy.get_param(
+            '~rfcomm_devices', '/var/lib/robot/rfcomm_devices.yaml')
+        if os.path.exists(yaml_path):
+            with open(yaml_path) as yaml_f:
+                self.rfcomm_devices = yaml.load(yaml_f)
+            rospy.loginfo('{} is loaded.'.format(yaml_path))
+        else:
+            rospy.logerr('Cannot find {}'.format(yaml_path))
+
+    def release_devices(self):
+        subprocess.call(
+            ['sudo', 'rfcomm', 'release', 'all'],
+            stdout=PIPE, stderr=PIPE)
+
+    def bind_devices(self):
+        dev_num = 0
+        for dev in self.rfcomm_devices:
+            while 0 == subprocess.call(
+                    ['rfcomm', 'show', '{}'.format(
+                        dev_num)], stdout=PIPE, stderr=PIPE):
+                rospy.loginfo('/dev/rfcomm{} is already used.'.format(dev_num))
+                dev_num += 1
+            subprocess.call(
+                ['sudo', 'rfcomm', 'bind', str(dev_num), dev["address"]],
+                stdout=PIPE, stderr=PIPE)
+            rospy.loginfo(
+                'Device {}({}) is bound to /dev/rfcomm{}'.format(
+                    dev["name"], dev["address"], dev_num))
+            dev_num += 1
+
+
+if __name__ == "__main__":
+    rospy.init_node('rfcomm_bind')
+    app = RfcommBind()
+    app.release_devices()
+    app.bind_devices()