【ROS】ros::Time::now()ができない話

現象

次のようなノードを書いたときにros::Time::now().toSec()の値が0になってしまって困りました。

int main(){
ros::init(“test”);

ros::Time t_start = ros::Time::now();
ROS_INFO(“%lf”, t_start.toSec());

return 0;
}

ちなみにこの現象はrosparam set use_sim_time trueをしている時だけで、しかもプログラムの後半で使ったときは普通にコンピュータ時間が出てきました。うーん、謎。

解決

原因はよくわからなかったけど、解決策としてはROS_INFOの前に

ros::Duration delay(1.0);
delay.sleep();

のように待ち時間を設けるか、

while( t_start.toSec() <= 0 ) t_start = ros::Time::now();

のように値が得られるまで待つかすることでちゃんと値が出てきました。
おそらくsim_timeを使う(=bagファイルの時間を使う)ときはクロックが遅くなる?のかな。ノードが起動してあまりにも早くros::Time::now()を呼びすぎると失敗するみたいです。

【ROS】DS4コントローラを振動させる

やりたいこと

ROSを使うとプレステのコントローラ「DualShock4」を使ってロボットの操作が簡単にできます。しかし、コントローラの操作をロボットに伝える事例はたくさんあっても、コントローラを振動させる(フォースフィードバック)やり方でハマったのでご紹介します。

王道は「ds4drv」と「joy_node」を使う。でも…

DS4コントローラとROSの連携で最も一般的なのはコントローラ用のドライバ「ds4drv」とROSのjoyパッケージのノードである「joy_node」を使うやり方です。(こちらのサイトを参考にしました)
ROSでPS4のコントローラ(DualShock4)を使う方法

ところが、振動機能を試してみると失敗します。
以下は失敗事例です。
振動のテストにはfftestコマンドを使います。まずはds4drvを起動して、コントローラとペアリング

$ sudo ds4drv 
[info][controller 1] Created devices /dev/input/js0 (joystick) /dev/input/event11 (evdev) 
[info][bluetooth] Scanning for devices
[info][bluetooth] Found device DC:0C:2D:5C:86:2B
[info][controller 1] Connected to Bluetooth Controller (DC:0C:2D:5C:86:2B)
[info][bluetooth] Scanning for devices
[info][controller 1] Battery: 37%
[warning][controller 1] Signal strength is low (31 reports/s)

一番上の行にあるevdevのイベント番号を見ます。今回は/dev/input/event11となっています。続いてfftestを実行します。

$ fftest /dev/input/event11
Force feedback test program.
HOLD FIRMLY YOUR WHEEL OR JOYSTICK TO PREVENT DAMAGES

Device /dev/input/event11 opened
…
(中略)
…

Uploading effect #0 (Periodic sinusoidal) ... Error:: Function not implemented
Uploading effect #1 (Constant) ... Error: Function not implemented
Uploading effect #2 (Spring) ... Error: Function not implemented
Uploading effect #3 (Damper) ... Error: Function not implemented
Uploading effect #4 (Strong rumble, with heavy motor) ... Error: Function not implemented
Uploading effect #5 (Weak rumble, with light motor) ... Error: Function not implemented
Enter effect number, -1 to exit

Uploading effectで始まる行ですべてError: Function not implementedが出てしまっています。この状態で0~5の数字キーを押しても全く振動してくれません。

確認すべきこと

カーネルのバージョン

まず第一に、linuxカーネルのバージョンを確認しましょう。linuxカーネルとはOSの中核を担うプログラムのことです。ds4の振動機能はlinuxではカーネル4.10以上でしかサポートされていないので、それ未満の場合は更新する必要があります。

$ uname -r 
4.9.0-43-generic

linuxカーネルはapt-get upgrade等では自動更新されないので、手動で更新する必要があります。といってもapt-getを使って簡単に更新できます。(※バージョンによっては更新後にエラーが起こりLinuxの起動に問題が出ることもあるので注意が必要ですが、最悪CUIでログインできれば元のバージョンに戻すとは可能です。)
カーネルのアップデートは以下コマンドでできます。

sudo apt-get install linux-generic-hwe-16.04

ds4drvを使わない

前述の通りds4drvは振動機能に対応していません。Linuxカーネル4.10以降ではds4drvを使わなくてもBluetoothでPCとペアリングが可能です。ペアリングの方法は、ds4drvを使う場合と同じくPSボタンとshareボタンを5秒くらい長押しし、ds4の青いランプがチカチカッと光るようになったら、PCのBluetoothの設定から新規デバイス追加を行います。

よくあるBluetoothペアリング

ペアリングが完了したら何もしなくてもds4drvで接続したのと同じ状態になっています。ROSを使う際もそのままrosrun joy joy_nodeを実行すればジョイスティックの値を見ることが可能です。


この状態でもう一度fftestによる振動テストを実行してみましょう。

$ fftest /dev/input/event19
Force feedback test program.
HOLD FIRMLY YOUR WHEEL OR JOYSTICK TO PREVENT DAMAGES

Device /dev/input/event19 opened
…
(中略)
…

Setting master gain to 75% ... OK
Uploading effect #0 (Periodic sinusoidal) ... OK (id 0)
Uploading effect #1 (Constant) ... Error: Invalid argument
Uploading effect #2 (Spring) ... Error: Invalid argument
Uploading effect #3 (Damper) ... Error: Invalid argument
Uploading effect #4 (Strong rumble, with heavy motor) ... OK (id 1)
Uploading effect #5 (Weak rumble, with light motor) ... OK (id 2)
Enter effect number, -1 to exit
0
Now Playing: Sine vibration
Enter effect number, -1 to exit

OKとなっている、#0、#4、#5が使えます。数字を入力するとコントローラが振動します。やったね!。

pythonプログラムから振動

kharlashkin氏のpythonプログラムです。いろんな強さ&時間で振動させることができます。python2系で動作します。python3系で動かすときはself.ff_joy = open(file, "r+")の部分をself.ff_joy = open(file, "r+b")に変えるといいみたいです。他のプログラムやROSから使うときはpythonで振動させられるのは便利ですね。

import fcntl, struct, array, time

EVIOCRMFF = 0x40044581
EVIOCSFF = 0x40304580
LOG_CLASS_ON = False
TIME_DELTA = 250

class Vibrate:

    def __init__(self, file):
        self.ff_joy = open(file, "r+")

    def close(self):
        self.ff_joy.close()

    def new_effect(self, strong, weak, length):
        effect = struct.pack('HhHHHHHxHH', 0x50, -1, 0, 0, 0, length, 0, int(strong * 0xFFFF), int(weak * 0xFFFF))
        a = array.array('h', effect)
        fcntl.ioctl(self.ff_joy, EVIOCSFF, a, True)
        return a[1]
        id = a[1]
        return (ev_play, ev_stop)

    def play_efect(self, id):
        if type(id) == tuple or type(id) == list:
            ev_play = ''
            for i in id:
                ev_play = ev_play + struct.pack('LLHHi', 0, 0, 0x15, i, 1)
        else:
            ev_play = struct.pack('LLHHi', 0, 0, 0x15, id, 1)
        self.ff_joy.write(ev_play)
        self.ff_joy.flush()

    def stop_effect(self, id):
        if type(id) == tuple or type(id) == list:
            ev_stop = ''
            for i in id:
                ev_stop = ev_stop + struct.pack('LLHHi', 0, 0, 0x15, i, 0)
        else:
            ev_stop = struct.pack('LLHHi', 0, 0, 0x15, id, 0)
        self.ff_joy.write(ev_stop)
        self.ff_joy.flush()

    def forget_effect(self, id):
        if type(id) == tuple or type(id) == list:
            for i in id:
                fcntl.ioctl(self.ff_joy, EVIOCRMFF, i)
        else:
            fcntl.ioctl(self.ff_joy, EVIOCRMFF, id)

f = Vibrate("/dev/input/event14")
p = f.new_effect(1.0, 1.0, TIME_DELTA )
f.play_efect((p))
time.sleep(TIME_DELTA / 1000.0)
f.stop_effect((p))
f.forget_effect((p))

https://github.com/chrippa/ds4drv/issues/91から引用

今回は以上です。今後はこれを使って危険をユーザに知らせるアラートシステムを作っていきたいと思っています。振動させるROSノードが完成したらそちらも公開しようと思います。論文で大変なので時間があればですが…。

参考

【ROS】hokuyo3dパッケージで3次元LiDARに接続できなかった話

問題

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

やったこと

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

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

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

[追加]→[IPv4]から固定IPアドレスを設定
IPアドレスを192.168.0.10に設定

一度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とかだと接続できない。

正しい固定IPは192.168.0.xx(ただし、xx≠10)

また、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

【ROS】PointCloudで任意の(x,y)でのz座標を得る

はじめに

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

やりたいこと

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

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

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

①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を最初から勉強したい方はこちら。

【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。

【ROS】launchファイルでcsvファイルが出力されない問題

問題

実験用にデータをcsvファイル出力するROSノードを作ったのに、単体(rosrun)ではファイルが作られるのにroslaunchでは作られない。

#これは作られる
rosrun imu imu_node

#これだと作られない
roslaunch imu imu.launch
<?xml version="1.0"?>
<launch>

  <node pkg="imu" type="imu_node" name="imu_node" />

</launch>

ちなみにROSのバージョンはKineticだが、全バージョン共通だと思う。たぶん。

原因と解決法

原因はlaunchファイルのオプションにあった。デフォルトではノードの実行場所は$ROS_HOMEになっていて、これはデフォルトで「~/.ros」になっている。ここを見てみるとちゃんと作られているようだ。

aaa@bbb:~$ ls .ros/
imu.csv                             rospack_cache_10391301543023691066
log                                 rospack_cache_11134725904490598093
roscore-11311.pid                   rospack_cache_15154294043039909020
rosdep                              rospack_cache_16685232255138339616
rospack_cache_04149954776256916034

隠しフォルダになっていると面倒なので、保存場所を変えたいと思って調べてみた。解決方法は3つ。

解決法①

launchファイルにオプションを追加して保存場所を変える。

<?xml version="1.0"?>
<launch>

  <node pkg="imu" type="imu_node" name="imu_node" cwd="node"/>

</launch>

nodeタグの中にオプション「cwd=”node”」を追加すると保存場所がプログラムの場所になる。つまり「~/catkin_ws/devel/lib/imu」になる。余計にわかりづらい。

解決法②

環境変数「ROS_HOME」を変える。デフォルトでは「~/.ros」になっているのを次のようにすると変えることができる。

export ROS_HOME=/home/aaa

そのまま同じターミナルでlaunchファイルを実行すると指定したフォルダにcsvファイルが作られる。しかし問題点が2つあり、ひとつはターミナルを起動する度に毎回上のコマンドを打たなければならないこと(.bashrcに上記を追記すれば大丈夫だけど…)。もうひとつは指定したパスにログファイルも作られてしまうことだ。ホームディレクトリとかにするとlogフォルダが出来ているとなんとなく嫌な感じがする。

解決法③

調べたところ、上2つの方法しか見当たらなかった…。やっぱり一番良いのはノードのソースファイル自体で保存場所をフルパスで指定すること。今までは

std::ofstream fout("imu.csv");

のように書いていたところを

std::ofstream fout("/home/aaa/imu.csv");

にすることで事無きを得た。
launchファイルって意外と融通きかないみたいで悲しい。

ROSのTFとは?簡単に説明(サンプルコード付き)②

前回、2つの座標系間の位置関係が変化しない場合のTFの発信をしました。今回は常に位置関係が変化する時のTFについて説明します。

動的TF

ロボットを扱っていると、常に位置関係が変化する2つの座標系を表したい時がよくあります。例えば部屋から見たロボットの位置だったり、アームロボットの手先の位置を表すのにもTFを使います。今回は変化するTFをどう発信するか説明します。

といっても基本的な考え方は静的TFの時と同じです。まずはサンプルプログラムを載せます。

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

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

  tf::TransformBroadcaster odom_broadcaster;

  double t = 0;

  while(ros::ok()){
    ros::spinOnce();

    geometry_msgs::TransformStamped odom_trans;

    //現在時刻、親フレーム、子フレームの指定
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.header.frame_id = "map";
    odom_trans.child_frame_id = "base_link";

    //親フレームからみた子フレームの位置
    odom_trans.transform.translation.x = t;
    odom_trans.transform.translation.y = sin(3.14*t);
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(cos(3.14*t));

    //TFを一回発信
    odom_broadcaster.sendTransform(odom_trans);

    t += 0.01;

    loop_rate.sleep();
  }
}

順番に見ていくと、まず最初にtransform_broadcasterをインクルードします。

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

続いてmain関数の最初の4行目でgeometry_msgs::TransformStamped型の変数を作っています。そんでそのヘッダ部分に現在時刻と親フレーム、子フレームの情報を入れます。

geometry_msgs::TransformStamped odom_trans;

//現在時刻、親フレーム、子フレームの指定
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = "map";
odom_trans.child_frame_id = "base_link";

そのあとが今回の本題、TFの座標を入れる部分です。x,y,zと回転を入れます。回転はクオタニオン(四元数)で表現します。クオタニオン表現では3次元ベクトルとそのベクトル周りの回転量で姿勢を表しますが、2次元平面だけで考える場合はヨー角からクオタニオンを作ってくれる便利な関数があるので非常に楽です。

//親フレームからみた子フレームの位置
odom_trans.transform.translation.x = t;
odom_trans.transform.translation.y = sin(3.14*t);
odom_trans.transform.translation.z = 0.0;

//ヨー角からクオタニオンを作ってrotationに入れる
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(cos(3.14*t));

TFの準備ができたところで次の関数で発信します。

//TFを一回発信
odom_broadcaster.sendTransform(odom_trans);

これを一定時間ごとに繰り返すだけで、動的TFの発信は終わりです。
今回はsin曲線を描きつつ進行方向(=微分方向=cos)を向くようにしたのでこんな感じの軌跡ができるはずです。

発信されたTFをRVizで確認。fixed_frameをmapに変えるのを忘れずに!

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

ros::Time::now()を数値にしてcsvに書き込もうとしても同じ値が出る件

はじめに

実験とかのためにROSトピックの値をcsvファイルに出力したいことが良くあるんですが、ROSの時刻を出力するのにはまったので解決までのメモです。

問題点

ROSの時間を数値(double)に変換するには

double t = ros::Time::now().toSec();

でOK。そんでcsvファイルに出力する

#include <ros/ros.h>
#include <fstream>

int main(int argc, char** argv){
  ros::init(argc, argv, "record_to_csv");
  ros::NodeHandle nh;
  ros::Rate loop_rate(20);
  
  std::ofstream fout("data.csv");
  fout << "Time[s]" << std::endl;

  while(ros::ok()){

    double t = ros::Time::now().toSec();
    fout << t << std::endl;

    loop_rate.sleep();
  }
}

と思ったら、全部同じ値になってしまった!

Time
0
1529990000
1529990000
1529990000
1529990000
1529990000
1529990000
1529990000
...

慌てて色々試したところ、ROS_ERRORではちゃんと表示される模様。

ROS_INFO("%lf",ros::Time::now().toSec());
[ INFO] [1530608049.263032619]: 0.000000
[ INFO] [1530608049.566322356, 1529991633.616588554]: 1529991633.616589
[ INFO] [1530608049.566409625, 1529991633.616588554]: 1529991633.616589
[ INFO] [1530608049.617069613, 1529991633.667305116]: 1529991633.667305
[ INFO] [1530608049.667780029, 1529991633.717981430]: 1529991633.717981
[ INFO] [1530608049.718522891, 1529991633.768572181]: 1529991633.768572

ところがstd::coutでは出力されない。

std::cout << ros::Time::now().toSec() << std::endl;
0
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09
1.52999e+09

解決

表示がROS_INFOと違うので疑っていたが、やはり桁数の問題だった。ファイル書き込みではデフォルトで6桁以降は切り捨てられるようだ。ROS関係のほうを疑ったり、ros::Time(0)を試してみたりしたけど全然だめで、ros::Time::now().secとros::Time::now().nsecを読んで(どちらもuint型)合計するとかやってしまった…。
std::coutもfoutも次のようにすれば桁数の設定ができる。

fout << std::setprecision(20) << ros::Time::now().toSec() << std::endl;
0
1529991603.1314094067
1529991603.1314094067
1529991603.1819009781
1529991603.2322974205
1529991603.2829723358
1529991603.3336381912
1529991603.3842408657

プログラミング上手い人にとっては常識なのかも知れないけど、そんなこともあるのか。勉強になりました。

ROSのsensor_msgs:PointCloud2データの構造と扱い方

はじめに

研究でデプスセンサを使うことがよくあるのですが、デプスセンサからの送られてくるsensor_msgs::PointCloud2形式のデータの扱いについて困った事がありました。例えばあるxy座標でのz座標を取り出したり、ある範囲のデータだけ取り出したりするのが意外と難しかったので、つまずいたことをまとめておきます。

データ形式

ROSの公式ドキュメントを見ればデータ形式そのものはわかるのですが、意味を理解するのにちょっと時間がかかりました。

まず、ポイントクラウドのデータ本体は「data」にあります。これは8ビット(0~255)の数字の列になっていますが、このデータの読み方を定義しているのが「fields」です。ROSのトピックを見てみると、次のようなデータになっていることがわかります。

rostopic echo /point_cloud/fields
- 
  name: "x"
  offset: 0
  datatype: 7
  count: 1
- 
  name: "y"
  offset: 4
  datatype: 7
  count: 1
- 
  name: "z"
  offset: 8
  datatype: 7
  count: 1
--

fieldsは配列になっていて、各データの見方を表しています。
各fieldの見方について説明すると、

  • [name]:値の名前
  • [offset]:先頭から何番目に始まっているか
  • [datatype]:データ型
  • [count]:何個のデータが入っているか(?)

を表します。つまりデータの入れ方は次のようになります。

data[]のデータ構造

point_stepは1つの点を何個のデータで表すか、で今回は12となります。
さて、xyz各データの読み方ですが、これはfieldのdatatypeで読むことになります。ドキュメントによると今回のデータ型「7」はfloat32を意味するとのことなので、この4バイトをfloat型で読み込めばいいということになります。(4バイトをfloatにする方法はネットに資料が沢山ありますので割愛します。)

data[]とfields[]以外のデータは主にポイントクラウドの大きさなどを表す情報で、

  • [height]:高さ
  • [width]:幅
  • [is_bigendian]:datatypeがビッグエンディアンかどうか
  • [point_step]:1つの点を何個のデータで表すか
  • [row_step]:1行を何個のデータで表すか
  • [is_dense]:すべての値が正常かどうか

などがあります。

ROSの勉強…

rosbagの便利オプション

実験時のセンサデータとかをすべて記録しておけるrosbagはROSの便利機能の中でもかなり有用な機能です。センサが手元に無くても記録したデータを再生することで、システムがちゃんと動作するか確認することが出来ます。
一方でrosbagには独自の使い方があったりするので、ここで一度出来ること&出来ないことを確認してみたいと思います。

事前に今流れているトピックを確認する

rostopic list

まず最初に記録すべきトピック、記録しなくてもいいトピックを見極めましょう。

record関連

rosbag record /xxx /yyy

特定のトピックだけを狙って記録する方法です。すべてのデータを記録するとファイルサイズがGB単位になって大変な時はこのやり方もありです。
しかし、あとになってこのデータ取っておけば良かった、となること多しなので慎重に決めましょう。

rosbag record -a

すべてのトピックを記録する方法です。後で完全にシミュレート出来ますが、ファイルサイズが膨大になります。小さなシステムならむしろおすすめです。再生したときにTFが上手く表示されない時は、下のplay –clockを使いましょう。

play関連

rosbag play test.bag -l
または
rosbag play test.bag --loop

bagファイルをリピート再生します。センサデータを使ってシステムのチェックを行いたい時等、何回も再生しなおすのが面倒な時に便利です。ちなみに通常の方法でも同様ですが、再生中にスペースキーを押すとポーズが出来ます。

rosparam set /use_sim_time true
rosbag play test.bag --clock

TFなどが上手く表示されない時に使います。一行目でroscoreのクロックを無効にし、二行目でバグファイルの時間を基準の時間として再生します。

rosbag play -r 0.5 test.bag

データを0.5倍速で再生します。データをゆっくり見たいときに。

rosbag play -s 45 -u 10 test.bag

45秒からスタートして10秒間再生します。一部だけ集中して見たい時や途中から見たいときに使います。-rや-lと合わせて使うことも多いです。

rosbag play test.bag tf:=tf_old

tfという名前のトピックをtf_oldにリネームして再生します。同じ名前のトピックがあってエラーが起きるときに使います。

検証関連

rosbag info test.bag

バグファイルにどんなトピックが記録されているか等の情報を見ることが出来ます。

ROS初心者の方はまずはこちらの本がオススメです。