【ROS】sensor_msgs::PointCloud2を座標変換する

はじめに

こんにちは。最近それこそVelodyneに近い性能を持って低価格なLRFが出てるみたいですね。自動車にもロボットにもLRFが大活躍してる現状を見ると、LRFの低価格化は嬉しいニュースです。

さてVelodyne等で取ったポイントクラウドを処理したい時に、各点をxyz座標で表したい時ってありますよね。LRFから出るデータはセンサ座標系でのxyz座標なので、他の基準座標系（例えばロボットの中心）からみたxyz座標に変換する必要があります。

ポイントクラウドの変換方法はpclを使う方法やsensor_msgsのライブラリを使う方法等、色々なやり方があって非常にわかりづらいです。今回は僕が使ってみて一番実装が簡単だと思ったやり方をご紹介します。

やり方

とりあえずサンプルコードはこちらです。

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

tf::StampedTransform transform;  // base_linkとbase_laserの座標変換を保存する用

void pc_callback(sensor_msgs::PointCloud2 pc2){
	sensor_msgs::PointCloud2 pc2_transformed;

	// 座標変換はこの一行だけ！
	pcl_ros::transformPointCloud("base_link", transform, pc2, pc2_transformed);

	// 確認用にxyz座標のsensor_msgs::PointCloudに変換
	sensor_msgs::PointCloud pc;
	sensor_msgs::convertPointCloud2ToPointCloud(pc2_transformed, pc);

	// xyzの値を表示
	for(int i=0; i<pc.points.size(); i++){
		ROS_INFO("x:%f y:%f z:%f", pc.points[i].x, pc.points[i].y, pc.points[i].z);
	}
}

int main(int argc, char** argv){
	ros::init(argc,argv,"transform_test");
	ros::NodeHandle nh;
	ros::Rate loop_rate(10);

	ros::Subscriber sub = nh.subscribe("in", 1, pc_callback);
	ros::Publisher pub = nh.advertise("out", 1);

	// TFを受信するやつ
	tf::TransformListener listener;

	// base_linkとbase_laserの間のTFが取得できるまで繰り返す
	while(true){
		try{
			//取得できたらtransformに保存しておく
			listener.lookupTransform("base_link", "base_laser", ros::Time(0), transform);
			ROS_INFO("I got a transform!");
			break;
		}
		catch(tf::TransformException ex){
			ROS_ERROR("%s",ex.what());
		  ros::Duration(1.0).sleep();
		}
	}

	//メインループでは特に何もしない
  while(ros::ok()){
  	ros::spinOnce();

  	loop_rate.sleep();
  }

	return 0;
}

まずは最初のincludeの部分ですが、

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

pcl_ros/transforms.hをインクルードします。pcl_rosのライブラリはROSのインストールに自動で入ってると思いますが、CMakeList.txtに書いておかないと見つけられないので注意してください。

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf
  pcl_ros
)

続いてグローバル変数でtransformを保存しておく用の変数を定義しています。

 tf::StampedTransform transform;  // base_linkとbase_laserの座標変換を保存する用

続いてトピックをsubscribeした時に呼ばれるコールバック関数です。ここで座標変換を行います。

void pc_callback(sensor_msgs::PointCloud2 pc2){
	sensor_msgs::PointCloud2 pc2_transformed;

	// 座標変換はこの一行だけ！
	pcl_ros::transformPointCloud("base_link", transform, pc2, pc2_transformed);

pcl_rosのリファレンスを見ると他にも色々な型or変換方法があるみたいです。今回はTFは一回取得するだけであとはずっと同じものを使うので、処理が軽そうなtransformを使う方法を取りました。この一行でポイントクラウドがbase_link座標系から見た値に変換されます。

その下は値の確認用です。現在のROSポイントクラウドの型の主流はsensor_msgs::PointCloud2型で、sensor_msgs::PointCloud型はあまり使われてない印象ですが、sensor_msgs::PointCloudのほうがxyz座標での表示で直感的にはわかりやすいのでよく使うんですよね。
他の方によるとsensor_msgs::PointCloud2型からsensor_msgs::PointCloud型への変換処理は重いらしいのでリアルタイムな処理をするときは注意が必要です。

// 確認用にxyz座標のsensor_msgs::PointCloudに変換
	sensor_msgs::PointCloud pc;
	sensor_msgs::convertPointCloud2ToPointCloud(pc2_transformed, pc);

	// xyzの値を表示
	for(int i=0; i<pc.points.size(); i++){
		ROS_INFO("x:%f y:%f z:%f", pc.points[i].x, pc.points[i].y, pc.points[i].z);
	}

続いてメイン関数です。

int main(int argc, char** argv){
	ros::init(argc,argv,"transform_test");
	ros::NodeHandle nh;
	ros::Rate loop_rate(10);

	ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("in", 1, pc_callback);
	ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("out", 1);

ここは通常のノードハンドルの宣言やSubscriberとPublisherの宣言です。公式ドキュメントそのまんまなので説明は割愛します。

続いてTFの受信です。

// TFを受信するやつ
	tf::TransformListener listener;

	// base_linkとbase_laserの間のTFが取得できるまで繰り返す
	while(true){
		try{
			//取得できたらtransformに保存しておく
			listener.lookupTransform("base_link", "base_laser", ros::Time(0), transform);
			ROS_INFO("I got a transform!");
			break;
		}
		catch(tf::TransformException ex){
			ROS_ERROR("%s",ex.what());
		  ros::Duration(1.0).sleep();
		}
	}

今回はTFは最初の一回だけ受信して、以降は同じ値をずっと使って座標変換します。TFの取得は結構注意が必要で、try-catchを書いておかないとすぐエラーで止まったりします。特にノード起動直後はエラーが起こりやすいので、ちゃんと例外処理しましょう。
lookupTransformは”base_link”から”base_laser”までのTFを取得していますが、順序が逆でも座標変換はできます。

//メインループでは特に何もしない
  while(ros::ok()){
  	ros::spinOnce();

  	loop_rate.sleep();
  }

最後のmainループでは特に何もしていません。トピックが来たらコールバック関数が実行されます。

結果

テスト用にダミーのポイントクラウドを生成してみました。base_linkの上1.0mにbase_laser座標系を置き、センサに水平に点を置いてみました。上のノードを動かした結果、ちゃんとbase_link座標系から見た値（z座標が1.0）になっています。

センサに水平にポイントクラウドを並べる
[ INFO] [1539448429.565021857]: x:0.808028 y:0.589145 z:1.000000
[ INFO] [1539448429.565045002]: x:0.777573 y:0.628793 z:1.000000
[ INFO] [1539448429.565095699]: x:0.745174 y:0.666870 z:1.000000
[ INFO] [1539448429.565120954]: x:0.710914 y:0.703279 z:1.000000
[ INFO] [1539448429.565142811]: x:0.674876 y:0.737931 z:1.000000
[ INFO] [1539448429.565163881]: x:0.637151 y:0.770739 z:1.000000
[ INFO] [1539448429.565184455]: x:0.597834 y:0.801620 z:1.000000
[ INFO] [1539448429.565205922]: x:0.557023 y:0.830497 z:1.000000
[ INFO] [1539448429.565231879]: x:0.514819 y:0.857299 z:1.000000
[ INFO] [1539448429.565282451]: x:0.471328 y:0.881958 z:1.000000
[ INFO] [1539448429.565307669]: x:0.426660 y:0.904412 z:1.000000
[ INFO] [1539448429.565329580]: x:0.380925 y:0.924606 z:1.000000
[ INFO] [1539448429.565350394]: x:0.334238 y:0.942489 z:1.000000
[ INFO] [1539448429.565372862]: x:0.286715 y:0.958016 z:1.000000
[ INFO] [1539448429.565394970]: x:0.238476 y:0.971148 z:1.000000
[ INFO] [1539448429.565470539]: x:0.189641 y:0.981854 z:1.000000
[ INFO] [1539448429.565500795]: x:0.140332 y:0.990105 z:1.000000
[ INFO] [1539448429.565522796]: x:0.090672 y:0.995881 z:1.000000
[ INFO] [1539448429.565544612]: x:0.040785 y:0.999168 z:1.000000

今回は以上です。

質問・コメント・間違っている箇所があれば教えてください。
よろしくおねがいしますm(_ _)m。

コメントを残す

メールアドレスが公開されることはありません。 ※ が付いている欄は必須項目です

CAPTCHA