Roboclaw Controller搭載の差動2輪ロボットでオドメトリをしてみよう！

はじめに

Roboclaw Controllerと呼ばれるモータドライバ搭載の差動2輪ロボットのオドメトリを、ROSを用いて算出してみようと思います。公式推奨のオドメトリ算出コードがGitHubに公開されているので、このコードを用いて、実際にロボットを走行させてオドメトリが取れているか確認しようと思います。ちなみに、ROSの環境構築は書籍や別記事がたくさんあるので、ここでは言及しないです。

使用条件

モータドライバ：Roboclaw Controller 2×15
制御コントローラ：Ubuntu mate 16.04 LTS
使用パッケージ：https://github.com/sonyccd/roboclaw_ros

事前準備

• このパッケージはdiagnosticが必要なので、入れてない人は先に入れてください。

sudo apt-get install ros-kinetic-diagnostics

• このパッケージのシリアルポートは/dev/ttyACM0です。ROS環境下ではシリアルポートに権限付与させないとおそらく動きません。なので、以下のコマンドを実行するとシリアルポートに権限が付与できます。
  sudo chmod 666 /dev/ttyACM0
  しかし、これでは、抜き差しするたびに設定しなければいけないです。そのため、自動で権限付与するようにします。以下のURLにその方法が書いてあります。
  https://qiita.com/fujikawabata/items/9ecdf59d1293fb8686b1

注意

自分が使用しているロボットのハードウェアの構成上の問題なのか、実行開始から最大速度指令を出すと、シリアルエラーが発生し電源が落ちてしまう状況になっています。なので、加速度をつけることで徐々に指定の速度に収束するような走行方法に変更しています。既存コードで走行できれば大丈夫です。

roboclaw_node.py

# 変更前 (261行)
roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks)

# 変更後
roboclaw.SpeedAccelM1(self.address, 110, vr_ticks)
roboclaw.SpeedAccelM2(self.address, 110, vl_ticks)

加速度の値は適宜、変えてください。(自分は110当たりがちょうどいいと思ったため、この値にしています)

コマンドによる動作

以下のコマンドを実行することで、トピックメッセージがパブリッシュされます。

rostopic pub /cmd_vel geometry_msgs/Twist "[0.0, 0.0, 0.0]" "[0.0, 0.0, 0.0]"

ここで、"[0.0, 0.0, 0.0]" "[0.0, 0.0, 0.0]"は"[Twist.linear.x, Twist.linear.y, Twist.linear.z]" "[Twist.angular.x, Twist.angular.y, Twist.angular.z]"
を表しています。
差動2輪であれば、速度[m/s]はTwist.linear.xに、旋回角速度[rad/s]はTwist.angular.zに書き込めばいいです。(オムニホイールのような全方向移動ロボットだとまた変わります。)
なお、こちらの書き方に変えても実行可能です。
'{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'

Pythonコードによる動作

一定速度で走行するコードを実行してみます。

roboclaw_node/nodes/twist_pub.py

# !/usr/bin/env python
import rospy

from geometry_msgs.msg import Twist
import time

# Ctrl-cで終了するまで走行しつづける
if __name__=="__main__":
        control_speed = 0.5     # [m/s]
        control_turn = 1.0       # [rad/s]
        rospy.init_node('test_twist')
        pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        try:
                r = rospy.Rate(1)
                while not rospy.is_shutdown():
                        twist = Twist()
                        twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0
                        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
                        pub.publish(twist)
                        print(twist)
                        r.sleep()
        except Exception as e:
                print(e)
        finally:
                twist.linear.x = 0.0
                twist.angular.z = 0.0
                pub.publish(twist)

最後、catkin_makeしてください。

実行結果

rostopic echo odomによってodomメッセージを購読できます。

見事にodomメッセージに座標系が更新されていることが確認できますね。

最後に

オドメトリは初めて使ったので良い勉強になりました。