ラズパイで作るROS搭載ロボット：Joystickで遠隔操作してみよう

ROSを使えば遠隔操作が簡単

最近はWRSに向けたガチ開発で、記事更新をサボっているAI coordinator管理人の清水秀樹です。

本日はJoystickを使って、ROSを搭載したラズパイタンクを遠隔操作してみたいと思います。

というか備忘録です。（年取ってきたせいか、すぐコマンドを忘れるため。）

サンプルソースコードも記事最下部に紹介していますので、参考にして頂ければと思います。

準備するもの

ROS搭載ラズパイタンク（別にタンクである必要性はない。動かせるロボットであればなんでも良い）
ROS搭載PC
Joystick

ROS搭載PCにJoystickを接続して、遠隔操作でROS搭載ラズパイタンクを動かす感じです。

ラズパイにROSを搭載するやり方は以下の記事を参考にしてください。

ラズパイで作るROS搭載ロボット：Ubuntu mate + ROS Kineticのセットアップ手順

ラズパイタンクを手軽に作りたいなら以下の記事を参考にしてみてください。

ADRPTB8C制御基盤を使った本格的なロボット開発　for ラズパイ

PCにubuntu+ROSをインストールしたい方は以下の記事を参考にしてください。

ROS環境の準備：iMac2012にubuntu14.04をネイティヴ環境にインストールする方法

遠隔操作方法

実はこれ、ROSを使うことですごい簡単に遠隔操作が実装できます。

コマンドライン数行で遠隔操作ができるようになります。

まずはラズパイタンク（ロボット側のROS）側で以下のコマンドを入力します。

$ roscore

続いて別のターミナルを起動して、ラズパイタンク側のIPアドレスと起動するプログラムを以下の通り入力します。

$ export ROS_IP=192.168.xx.xx
$ rosrun ros_start xxxx.py

以上でラズパイタンク（ロボット）側の準備は完了です。

続いてJoystickを接続した遠隔操作するPC側の設定です。

以下の通りコマンドを入力します。

$ export ROS_MASTER_URI=http://192.168.xx.xx:11311
$ export ROS_IP=192.168.xx.xx
$ rosrun joy joy_node

１行目に指定するIPアドレスはラズパイタンク側を指定します。

２行目のIPアドレスは自分のIPアドレスを指定します。

この場合はPC側のIPアドレスですね。

３行目はjoy_nodeの指定です。

この際、エラーが表示される場合は、joy_nodeのドライバーがインストールされていないことが原因ですので、以下のコマンドでインストールしましょう。

$ sudo apt-get install ros-indigo-joystick-drivers

indigo部分は必要に応じてROSバージョン名に変更してください。

全てが成功すればjoystickでROS搭載ラズパイロボットが動くはずです。

参考情報

IPアドレスの調べ方

IPアドレスを調べる方法についてです。

以下のコマンドを入力することで、自分のIPアドレスを知ることができます。

$ ifconfig

Joystickの入力値の調べ方

プログラムを作成する上で、Joystickの入力値を詳細に知る必要があります。

どのボタンを押したらどんな値が返ってくるのか。

以下のコマンドを入力すればJoystickの入力値を確認することができます。

$ roscore
$ rosrun joy joy_node
$ rostopic echo /joy

上記３つのコマンドはそれぞれ別のターミナルから入力してください。

成功すると以下のような値がターミナルに表示され、Joystickのボタンを入力するたびに値の変化を確認することができます。

header:
  seq: 632
  stamp:
    secs: 1525353295
    nsecs: 402288286
  frame_id: ''
axes: [-0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, 0.0]
buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
---

Joystickのどのボタンをどう入力するとどんな値が設定されているのか分かるようになりますので、これらの値の変化を把握した上でプログラムソースを作成しましょう。

サンプルソースコード

Joystickで動かすことができるサンプルソースコードです。

ロボットアーム用とキャタピラ駆動用と紹介します。

適当に使ってみてください。

ただし、使用上の注意点があります。

このソースを使用することによるハードの故障等は、一切責任を負いかねますので、それを了承の上で使用してください。

なぜなら、私はサーボモーターを壊しました。

ソースを間違えることで、簡単にハードが壊れることがあるので十分気をつけてください。

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
import Adafruit_PCA9685
from Adafruit_MotorHAT import Adafruit_MotorHAT

pwm = Adafruit_PCA9685.PCA9685(address=0x60)
pwm.set_pwm_freq(60)


def joy_sarbo(joy_msg):
    #print float(joy_msg.axes[3])
    #print float(joy_msg.axes[4])
    #print int(joy_msg.axes[3])
    #print int(joy_msg.axes[4])

    #０〜１８０度で９０が真ん中

    #print joy_msg.buttons[2]
    a =  90*joy_msg.buttons[2]/1.3
    pos2 = 90 +int(a)
    #print pos0
    pulse_2 = (650-150)*pos2/180+150+(-10)
    #print pulse_5
    pwm.set_pwm(2, 0, pulse_2)

    b =  90*joy_msg.axes[1]
    pos3 = 130 + (int(b))
    print pos3
    if pos3 >= 130:
        pulse_3 = (650-150)*pos3/180+150+(-10)
        #print pulse_5
        pwm.set_pwm(3, 0, pulse_3)

    #肩　初期値を５０度にしている。
    c =  -90*joy_msg.axes[0]
    pos4 = 50 + (int(c))
    print pos4
    pulse_4 = (650-150)*pos4/180+150+(-10)
    #print pulse_5
    pwm.set_pwm(4, 0, pulse_4)



    a =  90*joy_msg.axes[3]
    pos5 = 90 + (int(a))
    print pos5
    pulse_5 = (650-150)*pos5/180+150+(-10)
    #print pulse_5
    pwm.set_pwm(5, 0, pulse_5)

    b =  90*joy_msg.axes[4]
    pos6 = 90 + (int(b))
    print pos6
    pulse_6 = (650-150)*pos6/180+150+(-10)
    #print pulse_5
    pwm.set_pwm(6, 0, pulse_6)

    c =  90*joy_msg.axes[3]
    pos7 = 90 + (int(c)/2)
    print pos7
    pulse_7 = (650-150)*pos7/180+150+(-10)
    #print pulse_5
    pwm.set_pwm(7, 0, pulse_7)



rospy.init_node('sarbo_joy_twist')
sub = rospy.Subscriber('joy', Joy, joy_sarbo, queue_size=1)
rospy.spin()

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
import Adafruit_PCA9685
from Adafruit_MotorHAT import Adafruit_MotorHAT

mh = Adafruit_MotorHAT(addr=0x60)
myMotor1 = mh.getMotor(1)
myMotor2 = mh.getMotor(2)

def joy_callback(joy_msg):
    #print joy_msg.buttons[0]
    #print float(joy_msg.axes[0])
    print joy_msg.axes[6]
    print joy_msg.axes[7]
    if joy_msg.buttons[0] == 1:
        v = 255
    else:
        v=180

    if joy_msg.axes[6] == 0 and joy_msg.axes[7] >0:
	    myMotor1.run(Adafruit_MotorHAT.FORWARD)
	    myMotor2.run(Adafruit_MotorHAT.FORWARD)
	    a = v
	    b = v
    elif joy_msg.axes[6] > 0 and joy_msg.axes[7] == 0:
	    myMotor1.run(Adafruit_MotorHAT.BACKWARD)
	    myMotor2.run(Adafruit_MotorHAT.FORWARD)
	    a = v
	    b = v
    elif joy_msg.axes[6] < 0 and joy_msg.axes[7] == 0:
	    myMotor1.run(Adafruit_MotorHAT.FORWARD)
	    myMotor2.run(Adafruit_MotorHAT.BACKWARD)
	    a = v
	    b = v
    elif joy_msg.axes[6] == 0 and joy_msg.axes[7] < 0:
	    myMotor1.run(Adafruit_MotorHAT.BACKWARD)
	    myMotor2.run(Adafruit_MotorHAT.BACKWARD)
	    a = v
	    b = v

    elif joy_msg.axes[6] < 0 and joy_msg.axes[7] > 0:
	    myMotor1.run(Adafruit_MotorHAT.FORWARD)
	    myMotor2.run(Adafruit_MotorHAT.RELEASE)
	    a = v
	    b = 0
    elif joy_msg.axes[6] > 0 and joy_msg.axes[7] > 0:
	    myMotor1.run(Adafruit_MotorHAT.RELEASE)
	    myMotor2.run(Adafruit_MotorHAT.FORWARD)
	    a = 0
	    b = v
    elif joy_msg.axes[6] < 0 and joy_msg.axes[7] < 0:
	    myMotor1.run(Adafruit_MotorHAT.BACKWARD)
	    myMotor2.run(Adafruit_MotorHAT.RELEASE)
	    a = v
	    b = 0
    elif joy_msg.axes[6] > 0 and joy_msg.axes[7] < 0:
	    myMotor1.run(Adafruit_MotorHAT.RELEASE)
	    myMotor2.run(Adafruit_MotorHAT.BACKWARD)
	    a = 0
	    b = v
    else:
	    myMotor1.run(Adafruit_MotorHAT.RELEASE)
	    myMotor2.run(Adafruit_MotorHAT.RELEASE)
	    a = 0
	    b = 0

    myMotor1.setSpeed(abs(int(a)))
    myMotor2.setSpeed(abs(int(b)))

rospy.init_node('joy_twist')
sub = rospy.Subscriber('joy', Joy, joy_callback, queue_size=1)
rospy.spin()

以上です。

それではまた。