ROSを始めよう その４

前回の内容

その３

今回の内容

今回はROSのプログラムをシミュレーターも交えて練習していきましょう。
参考：http://wiki.ros.org/ja/ROS/Tutorials/CreatingMsgAndSrv

シミュレーターgazeboを使ってみる

インストールと実行

$ sudo apt-get install ros-kinetic-kobuki-gazeboでkobukiというロボットのシミュレーターをインストールします。
注：ラズパイでgazeboを動かすのはかなりキツイと思います。
それから、このインストールがうまくいかない場合はその６で紹介するturtlebotの方を使ってみましょう。
$ roslaunch kobuki_gazebo kobuki_playground.launchと打つと、シミュレーターgazeboが立ち上がります。

Publisherでロボットを動かす

vel_bumper.py

#! /usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import BumperEvent


rospy.init_node('vel_bumper')
# Parameter(プライベートパラメータ)を取得する
vel_x = rospy.get_param('~vel_x', 0.5)
vel_ros = rospy.get_param('~vel_rot', 1.0)
# Twist型のTopicに書き出すPublisherの作成
pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10)

def callback(bumper):
    # ぶつかるとbumperが反応してバックする
    back_vel = Twist()
    back_vel.linear.x = -vel_x
    rate = rospy.Rate(10.0)
    # 0.5秒Publishを続ける
    for i in range(5):
        pub.publish(vel)
        rate.sleep()

# BumperEvent型のTopicを購読して、callbackを呼ぶSubscriberの作成
sub = rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, callback)

while not rospy.is_shutdown():
    vel = Twist()
    # キーボード入力を受け付ける
    direction = raw_input('f: forward, b: backward, l: left, r:right > ')
    # 入力に応じて並進速度や回転速度を変更する
    if 'f' in direction:
        vel.linear.x = vel_x
    if 'b' in direction:
        vel.linear.x = -vel_x
    if 'l' in direction:
        vel.angular.z = vel_rot
    if 'r' in direction:
        vel.angular.z = -vel_rot
    if 'q' in direction:
        break
    print vel
    rate = rospy.Rate(10.0)
    # 1秒Publishを続ける
    for i in range(10):
        pub.publish(vel)
        rate.sleep()

新しいターミナルでこのプログラムを実行してみましょう。
いつものように、chmod 755で実行可能にしてrosrunで実行します。
ちなみに、ロボットの座標系は基本的に前がx軸(赤)、左がy軸(緑)、上がz軸(青)となっています。
キーボードの入力に合わせてロボットが動き、壁にぶつかるとバックします。

また、速度についてはParameterでセットすることができます。
$ rosparam set /vel_bumper/vel_x 1.0のようにコマンドを使うか、実行時に$ rosrun ros_beginner vel_bumper.py _vel_x:=1.0のように引数として与えるかすればよいです。
また、roslaunchにまとめるのであれば、以下のようにすればよいです。

<roslaunch>
  <node pkg="ros_beginner" name="vel_bumper" type="vel_bumper.py">
    <param name="vel_x" value="1.0">
  </node>
</roslaunch>

他のロボットを動かす

ROSのメリットとして、同じプログラムで異なるロボットが動かせたりします。

例えば、以下のようにturtlebotを動かせます。
引数で、PublishするTopicを変更していますが、これは型が同じだからできることです。

$ rosrun turtlesim turtlesim_node
$ rosrun ros_beginner vel_bumper.py /mobile_base/commands/velocity:=/turtle1/cmd_vel _vel_x:=1.5

また、以下のようにPR2というロボットを動かせます。

$ sudo apt-get install ros-kinetic-pr2-simulator
$ roslaunch pr2_gazebo pr2_empty_world.launch
$ rosrun ros_beginner vel_bumper.py /mobile_base/commands/velocity:=/base_controller/command

独自のTopicやServiceを作る

自分でTopicやServiceを作りたい場合も出てくると思います。その作り方を見ていきましょう。

コマンドrosmsg

まず、ROSでやりとりするデータであるMessageを調べるコマンドを紹介します。型を調べるときに便利なので、覚えておきましょう。
$ rosmsg show geometry_msgs/TwistのようにやりとりするMessageの型を調べることができます。
$ rosmsg listで利用可能な全Messageが表示できます。

独自のTopic

ROSのMessageはmsgファイルというもので定義されています。
例えば、先ほどのgeometry_msgs/Twistについてみてみましょう。

$ roscd geometer_msgs`
$ cat msg/Twist.msg

で定義を見ることができます。

Twist.msg

# comment
Vector3 linear
Vector3 angular

どうやらVector3型のlinear、angularというデータを持つようですね。
このように、msgファイルを書くことで独自の型のMessageが定義でき、Topic通信ができます。

$ roscd ros_beginner
$ mkdir msg
$ cd msg

msgと名付けたディレクトリの中に、自作のmsgファイルは作りましょう。自作のものを使うためには、いろいろ設定が必要なのですが、次の独自のServiceのところでまとめて説明します。

独自のService

独自のServiceを作るには、srvファイルで定義する必要があります。

$ roscd ros_beginner
$ mkdir srv
$ cd srv

srvと名付けたディレクトリの中に、以下のようなファイルを作ってみましょう。

SetVelocity.srv

float64 linear_velocity
float64 angular_velocity
---
bool success

並進速度と回転速度を入力として成否を出力するように定義しました。

これを使うためにいろいろな設定をしていきます。
まず、ros_beginnerの下にあるpackage.xmlを編集します。
35行目massage_generationと
39行目message_runtimeをコメントアウトして有効化します。

次に、CMakeLists.txtの7行目からのfind_package内にmessage_genrationを追加します。
53行目からのadd_service_filesをコメントアウトし、srvファイルをSetVelocity.srvに変更します。(今回はありませんが、独自のTopicがある場合、add_message_filesをコメントアウトし、msgファイルを自作のものに変更します。)
67行目からのgenerate_messagesをコメントアウトします。

これで、$ rossrv show ros_beginner/SetVelocityで定義が参照できるようになるはずです。

最後に~catkin_wsで$ catkin_makeすれば、使用可能になります。

独自のServiceを使ってみる

先ほど作ったServiceを利用すると、「外部から指定できる速度の範囲があり、それによって速度がセットされたか無視されたかを返す」ようなことができます。

velocity_server.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from ros_start.srv import SetVelocity
from ros_start.srv import SetVelocityResponse

MAX_LINEAR_VELOCITY = 1.0
MIN_LINEAR_VELOCITY = -1.0
MAX_ANGULAR_VELOCITY = 2.0
MIN_ANGULAR_VELOCITY = -2.0

def velocity_handler(req):
    vel = Twist()
    is_set_success = True
    if req.linear_velocity <= MAX_LINEAR_VELOCITY and (
            req.linear_velocity >= MIN_LINEAR_VELOCITY):
        vel.linear.x = req.linear_velocity
    else:
        is_set_success = False
    if req.angular_velocity <= MAX_ANGULAR_VELOCITY and (
            req.angular_velocity >= MIN_ANGULAR_VELOCITY):
        vel.angular.z = req.angular_velocity
    else:
        is_set_success = False
    print vel
    if is_set_success:
        pub.publish(vel)
    return SetVelocityResponse(success=is_set_success)

if __name__ == '__main__':
    rospy.init_node('velocity_server')
    pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10)
    service_server = rospy.Service('set_velocity', SetVelocity, velocity_handler)
    rospy.spin()

velocity_client.py

#!/usr/bin/env python
import rospy
from ros_start.srv import SetVelocity
import sys

if __name__ == '__main__':
    rospy.init_node('velocity_client')
    set_velocity = rospy.ServiceProxy('set_velocity', SetVelocity)
    linear_vel = float(sys.argv[1])
    angular_vel = float(sys.argv[2])
    response = set_velocity(linear_vel, angular_vel)
    if response.success:
        rospy.loginfo('set [%f, %f] success' % (linear_vel, angular_vel))
    else:
        rospy.logerr('set [%f, %f] failed' % (linear_vel, angular_vel))

では実行してみましょう。
$ roslaunch kobuki_gazebo kobuki_playground.launchを実行
chmodで実行可能にして、$ rosrun ros_beginner velocity_server.py
chmodで実行可能にして、$ rosrun ros_beginner velocity_client.py 0.5 0.0

まとめ

今回はROSのプログラムをシミュレーターも交えて練習していきました。
次回はActionという通信を見ていきたいと思います。

その５