最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

ROS の tf 変換ツリーへのアクセス

ROS の tf システムは複数の座標系を追跡し、それらの関係をツリー構造に保持します。tf は配布されるため、ROS ネットワーク内の各ノードですべての座標系に関する情報を使用できます。MATLAB® では、この変換ツリーにアクセスすることができます。この例では、使用可能な座標系へのアクセス、それら座標系間の変換の取得、および任意の 2 つの座標系間での点、ベクトル、その他のエンティティの変換について説明します。

必要条件: ROS 入門ROS ネットワークへの接続

開始

ROS システムを初期化します。

rosinit
Initializing ROS master on http://bat800808glnxa64:33935/.
Initializing global node /matlab_global_node_00675 with NodeURI http://bat800808glnxa64:34929/

この例で現実的な環境を作成するために、exampleHelperROSStartTfPublisher を使用して複数の変換をブロードキャストします。変換はロボットに取り付けられたカメラを表します。

この変換ツリーには 3 つの座標系が定義されています。

  • ロボットのベース座標系 (robot_base)

  • カメラの取り付け位置 (mounting_point)

  • カメラの光学中心 (camera_center)

次の 2 つの変換がパブリッシュされています。

  • ロボットのベースからカメラの取り付け位置までの変換

  • 取り付け位置からカメラ中心までの変換

exampleHelperROSStartTfPublisher

3 つの座標系の視覚的表現は次のようになります。

ここで、各座標系の x、y、z 軸はそれぞれ赤、緑、青の線で表されています。座標系間の親子関係を、子からその親座標系を指す茶色の矢印で示します。

関数 rostf を使用して新規の変換ツリー オブジェクトを作成します。このオブジェクトを使用して、使用可能なすべての変換にアクセスしたり、異なるエンティティに変換を適用したりすることができます。

tftree = rostf
tftree = 
  TransformationTree with properties:

    AvailableFrames: {0x1 cell}
     LastUpdateTime: [0x1 Time]
         BufferTime: 10

オブジェクトが作成されると tf 変換の受信を開始し、その変換を内部でバッファー処理します。変数 tftree は、データ受信を続行するようにワークスペース内に保持しなければなりません。

少しの間一時停止して、すべての変換が受信されていることを確認します。

pause(1);

AvailableFrames プロパティにアクセスして、使用可能なすべての座標系の名前を確認できます。

tftree.AvailableFrames
ans = 3x1 cell array
    {'camera_center' }
    {'mounting_point'}
    {'robot_base'    }

カメラ、その取り付け位置、およびロボットの関係を表す 3 つの座標系が表示されます。

変換の受信

これで使用可能になった変換を検査することができます。いずれの変換も ROS の geometry_msgs/TransformStamped メッセージで記述され、並進と回転のコンポーネントをもちます。

取り付け位置とカメラ中心との関係を表す変換を取得します。関数 getTransform を使用して、これを行います。

mountToCamera = getTransform(tftree, 'mounting_point', 'camera_center');
mountToCameraTranslation = mountToCamera.Transform.Translation
mountToCameraTranslation = 
  ROS Vector3 message with properties:

    MessageType: 'geometry_msgs/Vector3'
              X: 0
              Y: 0
              Z: 0.5000

  Use showdetails to show the contents of the message

quat = mountToCamera.Transform.Rotation
quat = 
  ROS Quaternion message with properties:

    MessageType: 'geometry_msgs/Quaternion'
              X: 0
              Y: 0.7071
              Z: 0
              W: 0.7071

  Use showdetails to show the contents of the message

mountToCameraRotationAngles = rad2deg(quat2eul([quat.W quat.X quat.Y quat.Z]))
mountToCameraRotationAngles = 1×3

     0    90     0

取り付け位置を基準として、カメラ中心は z 軸方向に 0.5 m の位置にあり、y 軸を中心に 90 度回転しています。

ロボットのベースとカメラの取り付け位置の関係を検査するために、getTransform を再度呼び出します。

baseToMount = getTransform(tftree, 'robot_base', 'mounting_point');
baseToMountTranslation = baseToMount.Transform.Translation
baseToMountTranslation = 
  ROS Vector3 message with properties:

    MessageType: 'geometry_msgs/Vector3'
              X: 1
              Y: 0
              Z: 0

  Use showdetails to show the contents of the message

baseToMountRotation = baseToMount.Transform.Rotation
baseToMountRotation = 
  ROS Quaternion message with properties:

    MessageType: 'geometry_msgs/Quaternion'
              X: 0
              Y: 0
              Z: 0
              W: 1

  Use showdetails to show the contents of the message

取り付け位置はロボットのベースの x 軸方向に 1 m の位置にあります。

変換の適用

camera_center 座標系で定義された 3 次元の点があり、robot_base 座標系でのその点座標を計算するとします。

関数 waitForTransform を使用して、camera_centerrobot_base の座標系間の変換が使用可能になるまで待ちます。この呼び出しは、camera_center から robot_base へのデータを要する変換が有効になり、変換ツリーで使用可能になるまでブロックされます。

waitForTransform(tftree, 'robot_base', 'camera_center');

ここで、カメラ中心の座標系で位置 [3 1.5 0.2] にある点を定義します。次に、この点を robot_base 座標に変換します。

pt = rosmessage('geometry_msgs/PointStamped');
pt.Header.FrameId = 'camera_center';
pt.Point.X = 3;
pt.Point.Y = 1.5;
pt.Point.Z = 0.2;

変換ツリー オブジェクトに対し関数 transform を呼び出して、点座標を変換できます。この変換のターゲット座標系を指定します。この例では robot_base を使用します。

tfpt = transform(tftree, 'robot_base', pt)
tfpt = 
  ROS PointStamped message with properties:

    MessageType: 'geometry_msgs/PointStamped'
         Header: [1x1 Header]
          Point: [1x1 Point]

  Use showdetails to show the contents of the message

変換後の点 tfpt は次の 3 次元座標をもちます。

tfpt.Point
ans = 
  ROS Point message with properties:

    MessageType: 'geometry_msgs/Point'
              X: 1.2000
              Y: 1.5000
              Z: -2.5000

  Use showdetails to show the contents of the message

PointStamped メッセージの他に、関数 transform を使用すると、姿勢 (geometry_msgs/PoseStamped)、四元数 (geometry_msgs/QuaternionStamped)、点群 (sensor_msgs/PointCloud2) など他のエンティティを変換できます。

変換を保存する場合、その変換を関数 getTransform で取得できます。

robotToCamera = getTransform(tftree, 'robot_base', 'camera_center')
robotToCamera = 
  ROS TransformStamped message with properties:

     MessageType: 'geometry_msgs/TransformStamped'
          Header: [1x1 Header]
    ChildFrameId: 'camera_center'
       Transform: [1x1 Transform]

  Use showdetails to show the contents of the message

この変換を使用して、camera_center 座標系の座標を robot_base 座標系の座標に変換できます。

robotToCamera.Transform.Translation
ans = 
  ROS Vector3 message with properties:

    MessageType: 'geometry_msgs/Vector3'
              X: 1
              Y: 0
              Z: 0.5000

  Use showdetails to show the contents of the message

robotToCamera.Transform.Rotation
ans = 
  ROS Quaternion message with properties:

    MessageType: 'geometry_msgs/Quaternion'
              X: 0
              Y: 0.7071
              Z: 0
              W: 0.7071

  Use showdetails to show the contents of the message

変換の送信

新規の変換を ROS ネットワークにブロードキャストすることもできます。

robot_base 座標系を基準として、wheel 座標系のオフセットを記述するシンプルな変換があるとします。車輪は y 軸方向に -0.2 m、z 軸方向に -0.3 m の位置に取り付けられています。この車輪は、y 軸を中心に相対的に 30 度回転しています。

この変換に対応してそれを記述する geometry_msgs/TransformStamped メッセージを作成します。ソース座標系 wheelChildFrameId プロパティに配置されます。ターゲット座標系 robot_baseHeader.FrameId プロパティに追加されます。

tfStampedMsg = rosmessage('geometry_msgs/TransformStamped');
tfStampedMsg.ChildFrameId = 'wheel';
tfStampedMsg.Header.FrameId = 'robot_base';

変換自体は Transform プロパティで記述されます。これは、TranslationRotation を含みます。上記の値を入力します。

Rotation は四元数として記述されます。y 軸を中心とした 30 度の回転を四元数に変換するには、関数 axang2quat を使用できます。y 軸はベクトル [0 1 0] で記述され、30 度は関数 deg2rad でラジアンに変換できます。

tfStampedMsg.Transform.Translation.X = 0;
tfStampedMsg.Transform.Translation.Y = -0.2;
tfStampedMsg.Transform.Translation.Z = -0.3;

quatrot = axang2quat([0 1 0 deg2rad(30)])
quatrot = 1×4

    0.9659         0    0.2588         0

tfStampedMsg.Transform.Rotation.W = quatrot(1);
tfStampedMsg.Transform.Rotation.X = quatrot(2);
tfStampedMsg.Transform.Rotation.Y = quatrot(3);
tfStampedMsg.Transform.Rotation.Z = quatrot(4);

rostime を使用して現在のシステム時間を取得し、それを変換のタイムスタンプとして使用します。これにより受信側は、どの時点でこの変換が有効だったのかわかります。

tfStampedMsg.Header.Stamp = rostime('now');

関数 sendTransform を使用してこの変換をブロードキャストします。

sendTransform(tftree, tfStampedMsg)

新規の wheel 座標系も使用可能な座標系のリストに含まれています。

tftree.AvailableFrames
ans = 4x1 cell array
    {'camera_center' }
    {'mounting_point'}
    {'robot_base'    }
    {'wheel'         }

4 つの座標系すべての最終的な視覚的表現は以下のようになります。

wheel 座標系には、座標系の送信時に指定した並進および回転があることがわかります。

例のパブリッシャーの停止と ROS ネットワークのシャットダウン

例の変換パブリッシャーを停止します。

exampleHelperROSStopTfPublisher

ROS マスターをシャットダウンしてグローバル ノードを削除します。

rosshutdown
Shutting down global node /matlab_global_node_00675 with NodeURI http://bat800808glnxa64:34929/
Shutting down ROS master on http://bat800808glnxa64:33935/.