去年aliexpressの独身セールではいろいろ爆買いをしてたのですが、その中でノーブランドなUSBで接続できるコントローラーを買いました。

ja.aliexpress.com

このコントローラーは前回動かしたLidarセンサーを買ったときと同じタイミングで買いました。

supernove.hatenadiary.jp

僕は普段ゲームはマイクラをやるぐらいでコントローラーを使うようなゲームはしてません。

そう、まさにROSでゲームコントローラーを使いたくて買ってましたが半年ぐらい放置してましたｗ。

というわけで今回はこの積んでたコントローラーを使ってロボットカーを操作できるようにしてみました。

今回の成果物は以下のレポジトリのbacklog/step4ブランチで切っています。ちなみに直進と回転のメッセージが同時に配信されたときに直進の値関係なく回転するというバグがあったので、前回切ったブランチからrober.pyのロジックを変更しています。

github.com

実行環境

Ubuntu 20.04
ROS noetic

とりあえずメッセージを受け取ってみる

まずはROSでジョイスティックのメッセージを配信するパッケージをインストールします。

以下のコマンドでパッケージをインストールできます。本来は有名メーカーのコントローラーのROS用ドライバーをインストールする必要がありますが、ビルドエラーが発生するのと今回のコントローラーはなくても動かせたのでドライバーはインストールせずいきます。

sudo apt install ros-noetic-joy

それではジョイスティックを動かすノードを起動します。

以下のコマンドでroscoreを立ち上げます。

roscore

コントローラーをパソコンに接続したら別ターミナルを開いて以下のコマンドでジョイスティックノードを起動します。

rosrun joy joy_node

更に別ターミナルを開いて以下のコマンドを実行するとjoy_nodeから配信されるメッセージを購読できます。

rostopic echo /joy

ボタンやレバーを操作すると以下のようにリストの値の変化を確認できます。

ボタンやレバーを操作したときと離したときでそれぞれメッセージが配信されることが確認できます。

---
header: 
  seq: 1613
  stamp: 
    secs: 1648992334
    nsecs: 825536911
  frame_id: "/dev/input/js0"
axes: [-0.0, 1.0, 0.0, 0.0, 0.0, 0.0]
buttons: [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
---
header: 
  seq: 1614
  stamp: 
    secs: 1648992334
    nsecs: 841519585
  frame_id: "/dev/input/js0"
axes: [-0.0, 1.0, 0.0, 0.0, 0.0, 0.0]
buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
---
header: 
  seq: 1615
  stamp: 
    secs: 1648992334
    nsecs: 914614853
  frame_id: "/dev/input/js0"
axes: [-0.0, -0.0, 0.0, 0.0, 0.0, 0.0]
buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
---

Pythonでロボットをコントロールする

これで無事にコントローラーで値を取れるようになったので今度はPythonでロボットを操作してみます。

実装

先程確認したjoy_nodeからのメッセージを参考にしながらロボットの操作を実装すると以下のコードになります。

ファイル名は control_rober_joystick.pyとします。

#! /usr/bin/env python3
import rospy
from rospy.exceptions import ROSInterruptException
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist


class Contorller:
    def __init__(self):
        rospy.init_node('controller', anonymous=True)
        self.linear_x = 0
        self.angular_zr = 0
        self.angular_zl = 0
        self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
        self.sub = rospy.Subscriber('joy', Joy, self.joy_callback)
        rospy.on_shutdown(self.shutdown_callback)
        rospy.Timer(rospy.Duration(1.0), self.timer_callback)
    
    def joy_callback(self, joy_msg):
        # Get Joy Event
        self.linear_x = joy_msg.axes[1]
        self.angular_zr = joy_msg.buttons[1]
        self.angular_zl = joy_msg.buttons[3]

    def timer_callback(self, event):
        twist = Twist()
        twist.linear.x = int(self.linear_x) * 0.5
        twist.angular.z = int(self.angular_zl - self.angular_zr) * 0.5
        rate = rospy.Rate(10)
        for i in range(5):
            self.pub.publish(twist)
            rospy.loginfo("linear_x: %f, angular_z: %f", twist.linear.x, twist.angular.z)
            rate.sleep()

    def shutdown_callback(self):
        rospy.loginfo("Stop")
        rospy.sleep(1)


if __name__ == '__main__':
    try:
        rospy.loginfo("Start")
        Contorller()
        rospy.spin()
    except ROSInterruptException:
        pass

これまでのシリーズで書いてきたコードとよく似ていますが、いくつか解説したいと思います。

まずは、コントローラーからのイベントを受け取る方法ですが、以下のコールバック関数を使用します。

ジョイスティックのイベントがオブジェクトで格納されているのであとはそこから必要な値を受け取るようになっています。どの値を受け取るかどうかはコントローラーによって変わると思うので、実際に動かしながらjoy_nodeから配信されるメッセージを確認するのがいいかと思います。

    def joy_callback(self, joy_msg):
        # Get Joy Event
        self.linear_x = joy_msg.axes[1]
        self.angular_zr = joy_msg.buttons[1]
        self.angular_zl = joy_msg.buttons[3]

そして次にメッセージを配信するにあたり以下の1行は僕の中では割と工夫したことです。angularの指定はボタンを押したか押してないか(1か0)が値として格納されています。つまり片方が1であればもう片方は必ず0になるようになっています。仮に別のボタンのイベントを拾って両方反応したとしてもプラマイゼロになるので回転は0として送られるわけです。

twist.angular.z = int(self.angular_zl - self.angular_zr) * 0.5

lauchファイルを用意

今回のプログラムは先程動かしたjoy_nodeが動いていることでそこからメッセージを購読してロボットに動きを配信する挙動になっています。

つまりjoy_nodeを起動しているのが前提なので、プログラムとセットで起動できるようにlaunchファイルを用意します。

ファイル名は joystick_control.launchとします。

<launch>
    <node name="joystick" pkg="joy" type="joy_node" />
    <node name="control_rober" 
        pkg="my_rober" 
        type="control_rober_joystick.py" 
        output="screen"
    />
</launch>

動作確認

それでは動作確認です。

今回は遠隔でロボットを動かそうと思うので、以前の記事を参考にマスターとスレーブの設定をしておきます。

supernove.hatenadiary.jp

その後、マスターでroscoreを起動します。

roscore

ロボット側では以下のコマンドを実行して司令の入力待ち状態にしておきます。

rosrun my_rober rober.py

コントローラーをつないでいるPCでは以下のコマンドを実行してコントローラーを動かせるようにします。

roslaunch my_rober joystick_control.launch

実際にロボットを動かしてみたのがこちらの動画です。

ちゃんとコントローラーの操作に合わせて車輪の動きが変わっているのがお分かりいただけると思います。

ゲームコントローラーからロボットを動かせるようになった！
コードを改造して変なところにバグが出てることに気づくの時間かかったけどなんとか想定通りに動いてよかったよかった(ハードウェアのテストどうにか出来ないかな…#rosjp pic.twitter.com/tq4RMbylM8
— KMiura (@k_miura_io) 2022年4月6日