投稿者: ppdr

問題

北陽電機の3DLiDAR「YVT-35LX」をROSノードで動かそうとしたら、「connection failed」になって接続できない。

やったこと

環境：(多分ROSが動くLinux環境なら共通)
Ubuntu 16.04
ROS kinetic

まず最初にYVT-35LXに電源12VとLANケーブルを接続。ノートパソコンとLANケーブルでつなぐ。

UbuntuのWiFiマーク->接続を編集する…->Ethernet->IPv4
の項目を設定して、固定IPアドレスにした。←これがまずかった

一度WiFiマークをクリックして切断→接続で設定を反映させる。

hokuyo3dパッケージをインストール
ノードを起動させると…

sudo apt-get install ros-kinetic-hokuyo3d

rosrun hokuyo3d hokuyo3d
[ INFO] [1544185161.008533462]: Connecting to 192.168.0.10
[ERROR] [1544185161.010972849]: Connection failed
[ INFO] [1544185161.111372013]: Connection closed
[ INFO] [1544185161.111556411]: Communication stoped

接続できない…。

正しい方法

いろいろ試行錯誤して最終的には接続できるようになった。原因を検証してみた結果次のポイントがあった。

まず、Ubuntu側の設定でLANケーブル接続時の固定IPアドレスの設定は、192.168.0.10以外の192.168.0.xxにしなければならない。
ここが192.168.1.20とかだと接続できない。

(2020/08/12 追記)
当時は理解が不十分でしたが、「192.168.0.xxにしなければならない」は間違いで、固定IPとセンサIPが同一サブネットに属すということなので、ネットマスクの値によります。上記はネットマスクが24のときです。

また、hokuyo3dノードのパラメータ[~/ip]と
[~/port]はセンサ自体の設定を専用ソフトでいじったりしない限りはデフォルトで良い。

# IPアドレスを192.168.0.20に変更
rosrun hokuyo3d hokuyo3d _ip:=192.168.0.20

これは接続できない。

なお、ノードのパラメータは一度設定すると、明示的に変更されるかroscoreが一度終了するまで残る。ノードを止めただけではパラメータはリセットされないので心配な人はroscoreやroslaunchを一度すべて止めよう。

解決するまでに一日使った…
成功するとちゃんとデータ出てる

ppdr@Ubuntu:~$ rosrun hokuyo3d hokuyo3d
[ INFO] [1544185571.860202705]: Connecting to 192.168.0.10
[ INFO] [1544185571.863357229]: Connection established
[ INFO] [1544185571.863433355]: Communication started

はじめに

LiDARもデプスカメラもPointCloudでデータを扱いますが、コストマップや平面検出や法線推定は多くのライブラリがあるのに対して、あるxy座標でのz座標の値をプロットしたいと思った時にちょっと困ったのでその方法をご紹介します。

やりたいこと

z座標を求めるにあたり次のパターンがあると思います。

• (x,y,0)に最も近い点を求め、その点のz座標を使う
• 全ての点をxy平面上に投影したときの最も近い点を求め、その点のz座標を使う
• 周辺の点の平均値をとる
• 周辺の点の中央値をとる

最近傍点を見つける際に3次元で考えるか2次元で考えるかで少し変わってくるケースもあるかと思いますので、みなさんの環境に合わせてどちらが良いか選んでください。

CMakeList.txt

今回の内容にはpcl_rosの追加が必要です。

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf
  pcl_ros
)

3次元近傍点探索

近傍点の探索にはPCLライブラリを使います。サンプルコードはこちらです。

3次元で半径rで探索して一番近い点を返すやつ

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/kdtree/kdtree_flann.h>


void callback(sensor_msgs::PointCloud2 pc2){
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_nan (new pcl::PointCloud<pcl::PointXYZ>); // NaN値あり
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); // NaN値なし
  
  //sensor_msgs::PointCloud2からpcl::PointXYZに変換
  pcl::fromROSMsg(pc2, *cloud_nan);

  // NaN値が入ってるといろいろ面倒なので除去
  std::vector<int> nan_index;
  pcl::removeNaNFromPointCloud(*cloud_nan, *cloud, nan_index);

  // KD木を作っておく。近傍点探索とかが早くなる。
  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
  tree->setInputCloud(cloud);

  //近傍点探索に使うパラメータと結果が入る変数
  double radius = 0.1;  //半径r
  std::vector<int> k_indices;  //範囲内の点のインデックスが入る
  std::vector<float> k_sqr_distances;  //範囲内の点の距離が入る
  unsigned int max_nn = 1;  //何点見つかったら探索を打ち切るか。0にすると打ち切らない
  
  pcl::PointXYZ p;  //中心座標
  p.x = 0.5;
  p.y = 0.5;
  p.z = 0.0;

  //半径r以内にある点を探索
  tree->radiusSearch(p, radius, k_indices, k_sqr_distances, max_nn);
  
  if(k_indices.size() == 0) return;
  
  pcl::PointXYZ result = cloud->points[k_indices[0]];

  ROS_INFO("A nearest point of (0.5, 0.5) is...\nx: %lf, y:%lf, z:%lf", result.x, result.y, result.z);
  
}

int main(int argc, char** argv){
	ros::init(argc,argv,"z_at_xy");
	ros::NodeHandle nh;
	
	ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("in", 1, callback);
	
	ros::spin();
	
	return 0;
}

このコードは最も近い1点を見つけます。k_indicesに見つかった点のインデックスが入ります。周辺の点の平均値や中央値をとるなどの処理をしたい場合はmax_nnを2以上か0にしておくと指定の個数になるまで探します。

xy平面上での距離で近傍点探索

2次元バージョンはこんな感じで

void callback_2d(sensor_msgs::PointCloud2 pc2){
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_nan (new pcl::PointCloud<pcl::PointXYZ>); // NaN値あり
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); // NaN値なし
  
  pcl::fromROSMsg(pc2, *cloud_nan);

  // NaN値が入ってるといろいろ面倒なので除去
  std::vector<int> nan_index;
  pcl::removeNaNFromPointCloud(*cloud_nan, *cloud, nan_index);

  //x座標とy座標だけをコピーしたPointCloudを作る
  pcl::PointCloud<pcl::PointXY>::Ptr cloud2d (new pcl::PointCloud<pcl::PointXY>); // NaN値なし
  cloud2d->points.resize(cloud->size());
  for(int i=0; i<cloud->points.size(); i++){
    cloud2d->points[i].x = cloud->points[i].x;
    cloud2d->points[i].y = cloud->points[i].y;
  }

  //treeも2Dで作る
  pcl::KdTreeFLANN<pcl::PointXY>::Ptr tree2d (new pcl::KdTreeFLANN<pcl::PointXY>);
  tree2d->setInputCloud(cloud2d);

  //近傍点探索に使うパラメータと結果が入る変数
  double radius = 0.1; //半径r
  std::vector<int> k_indices; //範囲内の点のインデックスが入る
  std::vector<float> k_sqr_distances; //範囲内の点の距離が入る 
  unsigned int max_nn = 1; //何点見つかったら探索を打ち切るか。0にすると打ち切らない
  
  //中心座標
  pcl::PointXY p;
  p.x = 0.5;
  p.y = 0.5;

  //2Dで近傍点探索
  tree2d->radiusSearch(p, radius, k_indices, k_sqr_distances, max_nn);
  
  if(k_indices.size() == 0) return;
  
  //求めたインデックスでもとのPointCloudの点を見る
  ROS_INFO("A nearest point of (0.5, 0.5) is...\nx: %lf, y:%lf, z:%lf", cloud->points[k_indices[0]].x, cloud->points[k_indices[0]].y, cloud->points[k_indices[0]].z);
  
}

int main(int argc, char** argv){
	ros::init(argc,argv,"z_at_xy");
	ros::NodeHandle nh;
	
	ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("in", 1, callback_2d);
	
	ros::spin();
	
	return 0;
}

pcl::PointCloudXYZのxy座標だけをコピーしたpcl::PointXYを作っておいてそちらで近傍点探索した後、得られたインデックスk_indicesでもとのpcl::PointCloudXYZを参照します。

求めた点の中央値を取る

平均値は求めた点のz座標を合計して個数で割るだけなのでたぶん簡単なので書く必要は無いと思いますが、中央値を取る方法は知らなかったので載せておきます。

ちなみに周辺の点の中央値をその点の値とする手法はメディアンフィルタ（中央値フィルタ）と呼ばれ、画像の境界線を残したままノイズを除去する方法として画像処理などでよく用いられるようです。このフィルタは計算コストがちょっと高いらしいので、多用はしないほうが良いかも知れません。

std::vector<float> vals;  //近傍点のz座標
for(int j=0; j<k_indices.size(); j++){
  vals.push_back(depth_cloud->points[k_indices[j]].z);
}

//上位半分までソートする
partial_sort(vals.begin(), vals.begin() + vals.size()/2 + 1, vals.end());

//中央値をとる
float filtered_z = vals[vals.size()/2];

ROSを最初から勉強したい方はこちら。

はじめに

こんにちは。最近それこそ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。