ROSでgopigo3を走らせる

#やること
raspberry pi3にubuntu mateをインストールして，ros kineticを用いてgopigo3を走らせます．
rosを使ってgopigoを走らせる記事や，githubのリポジトリなどで以下のようなものがあります．
https://github.com/ros-gopigo
https://codezine.jp/article/detail/9829
しかし，上記の2つはgopigo2のためのものであり，gopigo3で動作させようとすると通信はうまく行くけどなぜか動かない状況になります．
しかもエラーも出ないし，対処法も特にまとまってないので書いていこうと思います．

#ubuntu mateの準備
下記のリンク等を参考にインストールしてください．
インストールする際にユーザー名をpiにすると，gopigo3のドライバのインストールが楽です．
http://deviceplus.jp/hobby/raspberrypi_entry_048/

#ros kineticのインストール
以下のサイトを参考にインストールしていきます．
http://wiki.ros.org/ja/kinetic/Installation/Ubuntu

コマンドをまとめると以下のようになります．

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
sudo apt-get update
sudo apt-get install ros-kinetic-desktop
sudo rosdep init
rosdep update
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt-get install python-rosinstall

#gopigo3を動かせるように
まずI2Cを使えるように

sudo raspi-config
# 3のInterfacing Optionを選択
# P4 I2Cを選択
# enableする

下記を参考にgopigo3のドライバをインストールする
https://www.dexterindustries.com/gopigo3-tutorials-documentation/

sudo curl -kL dexterindustries.com/update_gopigo3 | bash
sudo reboot

cd ~/Dexter/GoPiGo3/Software/Python/Examples
python easy_Motors.py #これでモーターが動いたらインストール完了

ユーザー名がpi以外の人は上のやつではうまくいきません．
シェルスクリプトを書き換えないといけないので以下のようにしてスクリプトを実行するのではなくupdate_gopigo3.shに書き込みましょう．

sudo curl -kL dexterindustries.com/update_gopigo3 >> update_gopigo3.sh

update_gopigo3.sh

# Check for a GoPiGo3 directory under "Dexter" folder.  If it doesn't exist, create it.
PIHOME=/home/pi
DEXTER=Dexter
DEXTER_PATH=$PIHOME/$DEXTER
RASPBIAN=$PIHOME/di_update/Raspbian_For_Robots
curl --silent https://raw.githubusercontent.com/DexterInd/script_tools/master/install_script_tools.sh | bash

# needs to be sourced from here when we call this as a standalone
source /home/pi/$DEXTER/lib/$DEXTER/script_tools/functions_library.sh

GOPIGO3_DIR=$DEXTER_PATH/GoPiGo3
if folder_exists "$GOPIGO3_DIR" ; then
    echo "GoPiGo3 Directory Exists"
    cd $DEXTER_PATH/GoPiGo3    # Go to directory
    sudo git fetch origin       # Hard reset the git files
    sudo git reset --hard
    sudo git merge origin/master
    # change_branch $BRANCH
else
    cd $DEXTER_PATH
    git clone https://github.com/DexterInd/GoPiGo3
    cd GoPiGo3
    # change_branch $BRANCH  # change to a branch we're working on, if we've defined the branch above.
fi

sudo bash /home/pi/Dexter/GoPiGo3/Install/install.sh

上記のように/home/piとなっている部分を自分のホームディレクトリに書き換えましょう．スクリプト内で別のスクリプトを実行して行っているので，それらのスクリプトでも書き換えればインストールすることができます．

rosでモーターを動かす．

~/Dexter/GoPiGo3/Software/Pythonディレクトリにモーター制御のコードを書いていく

gopigo3_ros.py

#!/usr/bin/env python

import rospy
import math
from std_msgs.msg import Int16
from geometry_msgs.msg import Twist
import easygopigo3 as easy

gpg = easy.EasyGoPiGo3()

def callback(data):
    t = data.linear.x;
    r = data.angular.z;

    mag = math.sqrt(pow(t,2)+pow(r,2));

    if mag <= 0.1:
        motor1(0);
        motor2(0);
    else:
        m1 = 0;
        m2 = 0;

        m1 = t/mag * abs(t);
        m2 = t/mag * abs(t);

        m1 += r/mag * abs(r);
        m2 += -r/mag * abs(r);


        m1 = m1/abs(m1) * min(abs(m1), 1.0);
        m2 = m2/abs(m2) * min(abs(m2), 1.0);

        motor1(m1*255)
        motor2(m2*255)

def motor1(dps):
    gpg.set_motor_dps(gpg.MOTOR_RIGHT, dps)

def motor2(dps):
    gpg.set_motor_dps(gpg.MOTOR_LEFT, dps)

def listener():

    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("cmd_vel", Twist, callback)
    lwheel = rospy.Publisher("lwheel", Int16, queue_size=1)
    rwheel = rospy.Publisher("rwheel", Int16, queue_size=1)

    init_encoders = gpg.read_encoders()
    init_lwheel   = init_encoders[0]
    init_rwheel   = init_encoders[1]

    r = rospy.Rate(20)
    while not rospy.is_shutdown():
        encoders = gpg.read_encoders()
        lwheel.publish(encoders[0] - init_encoders[0])
        rwheel.publish(encoders[1] - init_encoders[1])
        r.sleep()

    motor1(0)
    motor2(0)


if __name__ == '__main__':
    listener()

次に3つコンソールを開く．
一つ目のコンソールで以下を実行．

roscore

2つ目のコンソールで以下を実行．

python gopigo3_ros.py

3つ目のコンソールで

rostopic pub /cmd_vel geometry_msgs/Twist '[0.2, 0.0, 0.0]' '[0.0, 0.0, 0.0]'

これで直進したら成功，回転させたいときは以下のコマンド．

rostopic pub /cmd_vel geometry_msgs/Twist '[0.0, 0.0, 0.0]' '[0.0, 0.0, 0.2]'

あとはcmd_velをmove_baseから入力させるなり，自由にどうぞ．