ROS研究ノート：TF座標変換と詳細なプログラミング

その他 2020-10-14 20:15:00 訪問数: null

ROS研究ノート：TF座標変換と詳細なプログラミング

1.はじめにとTFの紹介

TFとは何ですか？

tfは、ユーザーが複数の参照システムを経時的に追跡できるようにする機能パッケージです。ツリータイプのデータ構造を使用して、複数の参照システム間の座標変換関係を時間に応じてバッファリングおよび維持します。ユーザーがいつでもポイントを設定できるようにします。ベクトルなどのデータの座標は、2つの参照システムで座標変換を完了します。

簡単に言えば、ロボットの作業中、ロボット自体はワールド座標系にあり、同時にローカル座標系を持っていることがわかります。これら2つの座標系が存在することで、ロボットの姿勢を計算できます。

しかし、ロボット本体の場合、ポーズを決める必要のあるパーツも多く、時間によって状態が異なる場合があります。ボトムフレームをベースマークとして使用する場合、どのように違いを判断しますか？リダー、グリッパー、ジョイント、ヘッドなど、ロボット内部の位置と姿勢はどうですか？最善の方法は、これらのものにそれぞれの座標系を確立し、座標変換を通じてポーズを取得することです。

TFの機能は、すべての座標系間でシステム内の任意の点の座標変換を実現するのに役立ちます。システム内の特定の点について、TFは他の座標系の点の座標を変換するのに役立ちます。
ここに写真の説明を挿入

2.詳細なTFプログラミング例

では、どのようにTFを適用するのでしょうか。最初にデモンストレーションとしてROSによって与えられた例を取り上げましょう。

公式TFチュートリアル

まず、TF機能パックを最初にインストールします

sudo apt-get install ros-melodic-turtle-tf

次のコマンドを実行します

roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key

キーボードコントロールを使用して中央のタートルを初期化して移動すると、別のタートルが自動的に追従するのを確認できます。
ここに写真の説明を挿入
このプロセスは、主にturtle1とturtle2の間の座標変換によって実現されます。合格できます

rosrun tf view_frames	//创建一个TF监听器监听5s，得到TF结构关系图
rqt_graph				//查看节点信息

システムで実行されているすべての座標系の関係構造図とノード情報図を取得します
ここに写真の説明を挿入
ここに写真の説明を挿入

2.1一般的なデータタイプ

ROS Wikiに記載されているTFデータタイプを参照してください。クォータニオン、ベクトル、ポイント座標、ポーズ、変換テンプレートに対応する、主に次の基本タイプがあります。
ここに写真の説明を挿入
さらに、tf :: Stampedも含まれています。公式のソースコードは次のとおりです。つまり、このデータタイプは、すべての基本タイプ（tf :: Transformを除く）に基づいて要素frame_id_およびstamp_でテンプレート化されます。
ここに写真の説明を挿入

tf :: StampedTransformもあります。これは、tf :: Transformの特殊なケースであり、frame_id、stamp、およびchild_frame_idが必要です。
ここに写真の説明を挿入
合計6つのデータタイプ。

2.2TFの使用方法

ここに写真の説明を挿入
TFパッケージを使用する場合、TF変換の監視とTF変換のブロードキャストの機能を実行するために、2つのプログラムを作成する必要があります。これらをTFリスナーとTFブロードキャスターと呼びます。

  • TFモニター：TF変換を監視し、システムで公開されているすべての参照システム変換を受信して​​キャッシュし、そこから必要な参照システム変換を照会します。
  • TFブロードキャスター：TF変換をブロードキャストし、システム内の参照システム間の座標変換関係をブロードキャストします。システム内の異なる部分の複数のtf変換ブロードキャストが存在する場合がありますが、各ブロードキャストは、同期せずに参照フレームの変換関係をtfツリーに直接挿入できます。

公式チュートリアルを例にとってみましょう。プログラムは、上記のタートルフォロー効果を実現します。

2.3TFブロードキャスター

実現機能：TFブロードキャスターを作成し、座標変換値を作成し、座標変換をリアルタイムで公開します

プログラミングのアイデア：

  1. ROSノードを初期化し、タートルの位置情報をサブスクライブします。
  2. トピックメッセージをループで待機し、受信後にコールバック関数に入ります。コールバック関数は、座標変換を処理および公開するために使用されます。
  3. コールバック関数内にブロードキャスターを定義します。
  4. リトルタートルの受信位置メッセージに基づいて座標変換値を作成します
  5. 定義された放送局を通じて座標変換を公開する

以下は、特定の実装のコード部分です

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void PoseCallBack(const turtlesim::PoseConstPtr& msg)
{
    
    
	//定义一个tf广播器
	static tf::TransformBroadcaster br;
	
	//根据turtle的位置信息，得到其相对于世界坐标系的变换
	tf::Transform transform;
	transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
	tf::Quaternion q;
	q.setRPY(0,0,msg->theta);
	transform.setRotation(q);
	
	//tf广播器发布2只海龟相对于世界坐标系的坐标变换
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    
    
	//初始化节点
	ros::init(argc, argv, "my_tf_broadcaster");
	
	//判断并且获取turtle_name
	if(argc != 2)
	{
    
    
		ROS_ERROR("Need a turtle name!");
		return -1;
	}
	turtle_name = argv[1];
	
	//订阅turtle的pose信息
	ros::NodeHandle n;
	ros::Subscriber sub = n.subscribe(turtle_name + "/pose", 10, &PoseCallBack);
	
	ros::spin();
	
	return 0;
}

コードの説明：

ここでtfパッケージを学習した学生は、ROS内部メッセージの公開とサブスクリプションに精通している必要があります。ここでは、主に、コールバック関数でのtf変換に関連するコードコンテンツについて説明します。C ++にあまり詳しくない一部の学生の場合、argcとargvの意味の詳細については、ここを参照してください。メイン関数のargcとargv

具体的な分析の前に、「transform_broadcaster.h」の内容を見てみましょう。

#ifndef TF_TRANSFORMBROADCASTER_H
#define TF_TRANSFORMBROADCASTER_H
 
#include "tf/tf.h"
#include "tf/tfMessage.h"
 
#include <tf2_ros/transform_broadcaster.h>
 
namespace tf
{
    
    
class TransformBroadcaster{
    
    
public:
  TransformBroadcaster();
 
  void sendTransform(const StampedTransform & transform);
 
  void sendTransform(const std::vector<StampedTransform> & transforms);
 
  void sendTransform(const geometry_msgs::TransformStamped & transform);
 
  void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
 
private:
 
  tf2_ros::TransformBroadcaster tf2_broadcaster_;
 
};
 
}
 
#endif //TF_TRANSFORMBROADCASTER_H

TransformBroadcasterクラスは名前名tf内で定義されていることに注意してください。このクラスの内部コンテンツも非常に単純です。

  • パラメータなしのコンストラクターTransformBroadcaster（）;を宣言しました。
  • 関数のオーバーロードを使用する方法では、同じ名前の複数の関数sendTransformを定義します。
  • プライベート化されたメンバー変数tf2_broadcaster_が宣言されています

この内容に基づいて、コールバック関数コードのレベルを次のように分割します。

（1）tfブロードキャスターを定義する

static tf::TransformBroadcaster br;

tf :: TransformBroadcasterには引数のないコンストラクターがあるため、初期化中にデフォルトのコンストラクターが直接呼び出され、tfブロードキャスターbrが宣言されます。

（2）座標変換を作成する

tf内のデータタイプに応じて、最初に「変換」データ構造を宣言して、変換コンテンツを記録します

tf::Transform transform;

このコンテンツでは、変換された姿勢、つまり位置と方向を決定する必要があります。Transformクラスで関数を探します：http：//docs.ros.org/melodic/api/tf/html/c++/classtf_1_1Transform.html

次の2つの機能は私たちのニーズを達成することができます。内部パラメータは、
ここに写真の説明を挿入
さらに検索するには「Vector3」と「Quaternion」である必要があり（興味がある場合は、公式Webサイトにアクセスして確認してください。ここでは繰り返しません）、Vector3タイプは宣言することで直接使用でき、Quaternionタイプは最初にsetRPYを使用する必要があることがわかりました。この関数は割り当てを実行します。RPYについては、UAVのヨー角、ロール角、ピッチ角の説明をご覧ください。
ここに写真の説明を挿入
これから、

transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
transform.setRotation(tf::Quaternion::setRPY(0, 0, msg->theta));

さらに単純化して取得します

tf::Transform transform;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
tf::Quaternion q;
q.setRPY(0,0,msg->theta);
transform.setRotation(q);

（3）放送局が座標変換を公開

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

前に説明した「transform_broadcaster.h」の内容によると、
ここに写真の説明を挿入
内部パラメータタイプが「StampedTransform」であることを理解するのは難しいことではありません。次の図は、継承図を示しています。これは、StampedTransformクラスの内容がTransformから継承されることを意味します。
tf :: StampedTransformの継承図
入力（つまり、必要な座標変換）、タイムスタンプ、フレームID、およびサブフレームIDを使用する場合は、内部で宣言する必要があります。
ここに写真の説明を挿入

2.4TFリスナー

実現機能：TFモニターを作成し、2番目のタートルを作成し、座標変換をモニターし、モーションコントロール命令を発行して、2番目のタートルを最初のタートルに移動させます。

プログラミングのアイデア：

  1. ROSノードを初期化し、ノード情報をMASTERに登録します。
  2. サービスコールを通じて2番目のカメをスポーンします。
  3. turtle2の速度制御パブリッシャーを作成します。
  4. tfリスナーを作成し、turtle1に対するturtle2の座標変換を監視します。
  5. 座標変換に従って速度制御コマンドを発行します。

以下は、特定の実装のコード部分です

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    
    
	//初始化节点
	ros::init(argc, argv, "my_tf_listener");
	ros::NodeHandle n;
	
	//service调用产生第二只海龟
	ros::service::waitForService("spawn");
	ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);
	
	//定义速度控制指令的消息发布者
	ros::Publisher turtle_vel = n.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
	
	//定义一个tf监听器
	tf::TransformListener listener;
	
	ros::Rate rate(10.0);
	
	while(ros::ok())
	{
    
    
		//申明一个空的坐标变换
		tf::StampedTransform transform;
		try
		{
    
    
			//监听turtle2和turtle1的坐标变换
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
		}
		catch(tf::TransformException &ex)
		{
    
    
			ROS_ERROR("%s", ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		//根据坐标变换计算得出turtle2的角速度和线速度，并发布该消息
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(),2) + pow(transform.getOrigin().y(),2));
		
		turtle_vel.publish(vel_msg);
		
		rate.sleep();
	}

	return 0;
}

ここでは主にtfの部分についてお話しますが、「transform_listener.h」の内容は多すぎるのでここでは入れません〜公式文書はご自身でご確認いただけます。

対照的に、wikiでリスナーを使用する方法を確認することもできます：http：//wiki.ros.org/tf/Overview/Using%20Published%20Transforms

（1）tfリスナーを定義する

まず、TransformListenerのコンテンツがTransformerクラスから継承されていることを明確にする必要があるため（これはTransformerクラスではないことに注意してください）、使用するときに両方のドキュメントを同時に確認する必要があります。もちろん、最も簡単なのはwikiでリスナーの使用を確認することです。
ここに写真の説明を挿入
内部構造とデストラクタは次のとおりです（ヘッダーファイルから取得）。
ここに写真の説明を挿入
ほとんどの場合、次のコマンドを使用して宣言します。

tf::TransformListener listener

（2）座標変換を監視する

最初に空の変換を宣言します

tf::StampedTransform transform;

では、なぜここで宣言するためにtf :: Transformの代わりにtf :: StampedTransformを使用するのですか？メッセージを公開するためにtfブロードキャスターが使用するデータタイプについて慎重に考えてみましょう。

試してみてください

listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);

Wikiは次のように説明しています。

①tf:: TransformListener :: waitForTransform関数は、source_frameを一度にtarget_frameに変換できるかどうかをテストし、変換を実行できるかどうかを示すboolタイプの値を返します。
ここに写真の説明を挿入
②関数tf :: TransformListener :: lookupTransformは、2つの座標系間の変換を返し、返される変換方向はtarget_frameからsource_frameです。
ここに写真の説明を挿入
（3）何か問題が発生した場合はどうすればよいですか？

すべてのtf例外クラスはtf :: TransformExceptionを継承し、tf :: TransformException自体はstd :: runtime_errorを継承します。
ここに写真の説明を挿入
ここに写真の説明を挿入
次に、次のコードの内容は、what（）を使用して、エラーメッセージがキャッチされたときにエラーの基本情報を抽出し、エラーメッセージを出力することです。その後、システムは一定時間ハングし、ループを続行します。

ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;

3効果を達成する

プログラムを作成した後、CMakeLists
ここに写真の説明を挿入
ファイルにLaunchファイルを追加して構成します

<launch>
	<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

	<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />

	<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
	<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

	<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />

</launch>

コマンドラインで起動ファイルを実行すると、効果図は次のようになります。
ここに写真の説明を挿入
この時点で、rqt_graphを使用してノード情報を表示し、tf関数パッケージを使用した場合と基本的に同じであることがわかります。
ここに写真の説明を挿入
これで、tfの基本的なプログラミング学習コンテンツは完了です。

参照

ROSにおけるTFの基本的な役割との簡単な紹介
ROS探査の概要（18）-reread TF
http://wiki.ros.org/tf
http://docs.ros.org/jade/api/tf/html/c++/ namespacetf.html
http://wiki.ros.org/roscpp/Overview/TimeROS-TF
ライブラリ-ROSロボットのTFおよびros :: Time（0）
tf変換

再印刷のソース情報を示してください

おすすめ

転載: blog.csdn.net/moumde/article/details/104849253
おすすめ
ランキング
Oracleのクエリ重複フィールド
An error occurred when ssm used count to query data
【Leyes de la Naturaleza】La sabiduría de las multitudes
JavaWebの研究では、（13）を締結 - セッションの使用は、重複送信フォームを防ぎます
Firebase増加サインアップクォータ
[MyBatisフレームワーク]mybatis入門
ハートレスの世界
Djangoのインストールと使用について
[转] UiPath展開アーキテクチャ
mybatis-plusは楽観的なロック変更を使用します
アーカイブ
もっと
2024-05-14(9)
2024-05-13(8)
2024-05-12(27)
2024-05-11(31)
2024-05-10(33)
2024-05-09(30)
2024-05-08(18)
2024-05-07(34)
2024-05-06(6)
2024-05-05(0)

コードワールド

© 2018 codetd.com