ROSを始めよう その８

前回の内容

その７

今回の内容

今回は座標変換ツールtfなどを見ていきましょう。
参考：http://wiki.ros.org/ja/tf/Tutorials
　　　http://wiki.ros.org/ja/urdf/Tutorials/Create%20your%20own%20urdf%20file
　　　http://wiki.ros.org/ja/pcl/Tutorials

座標変換ツールtf

ロボットアームなどを動かす時には、関節角度から手先の位置を求める順運動学の問題などを解く必要があります。tfはこれを手助けしてくれるツールです。

PR2を動かしてみる

$roslaunch pr2_gazebo pr2_empty_world.launchでシミュレーターを立ち上げる。
次に動かす際に便利なライブラリを取ってきます。(まだリリースされていないソフトです。)

$ cd ~/catkin_ws/src
$ git clone https://github.com/OTL/ez_utils.git
$ cd ~/catkin_ws
$ catkin_make

そして$ rosrun rviz rvizとrvizを立ち上げて、Robot Modelを表示します。(表示されない場合、画面左のGlobal Optionsをbase_linkにセットしましょう。)
さらに、TFをAddして表示しましょう。(画面左のTFのFramesのAll Enabledをクリックして表示を外して見やすくしましょう。)
TFのhead_plate_frameとl_gripper_led_frameにチェックを入れると、PR2の頭と左手先のフレームが表示されます。

$ rosrun tf tf_echo /head_plate_frame /l_gripper_led_frameと打つと、/head_plate_frameから見た/l_gripper_led_frameの座標系、すなわち相対位置を得られます。

常に頭が左手先を見続けるプログラムを作ってみる

look_hand.py

#!/usr/bin/env python
import rospy
import tf2_ros
from ez_utils.ez_joints import JointsServer

if __name__ == '__main__':
    rospy.init_node('pr2_look_left_hand')
    # TFのデータを蓄えるバッファを作成
    tf_buffer = tf2_ros.Buffer()
    # TransformListenerを作成
    tf_listener = tf2_ros.TransformListener(tf_buffer)
    # 頭と左腕をTopicで動かせる状態にする
    head = JointsServer('/head_traj_controller')
    left_arm = JointsServer('/l_arm_controller')
    # 首の角度の初期化
    yaw_angle = 0.0
    pitch_angle = 0.0
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            # frame間の相対関係を取得
            trans = tf_buffer.lookup_transform('head_plate_frame',
                                               'l_gripper_led_frame',
                                               rospy.Time())
            # 首の角度を決定
            yaw_angle = trans.transform.translation.y / 1.0
            pitch_angle = -trans.transform.translation.z / 1.0
            print trans.transform.translation
            # 角度をロボットに送信
            head.set_positions([yaw_angle, pitch_angle])
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, 
                tf2_ros.ExtrapolationException):
            rospy.logwarn('tf not found')
        rate.sleep()

chmodで実行可能にし、$ rosrun ros_beginner look_hand.pyで実行しましょう。

rosrun rqt_ez_publisher rqt_ez_publisherでrqt_ez_publisherを立ち上げ、左手先を動かしてみましょう。
Topicとして/l_arm_controller/follow_positionを選択して追加し、スライダーで動かすことができます。

その他の基本的なライブラリ

ロボット定義フォーマットURDF

ROSで自作ロボットを使う場合などに、ロボットの見た目やモーターの配置、センサーの配置を定義するファイルが必要になります。これがURDFです。
参考：http://wiki.ros.org/urdf

3次元点群処理PointCloud Library

3次元点群を扱った認識プログラムを書く際に縁なライブラリとしてpcl_rosがあります。
参考：http://wiki.ros.org/pcl_ros

InteractiveMarkers

rvizを使って入力をするためのツールです。
参考：http://wiki.ros.org/interactive_markers

まとめ

今回は座標変換ツールtfなどを見ていきました。
次回は、Arduinoなどによる自作ロボットの動かし方を見ていきましょう。

その９