【ROS】TFのbroadcast,lookupと型変換まとめ(C++)

目次

C++におけるTF型

ROSで使うTF型は一つではなく、TF,TF2,geometry_msgsの3つのクラスが使われている。TFクラスは古いため現在使用が推奨されていないので、新しいクラスであるTF2を可能な限り使う。

よく使うTFメッセージは以下のようなものがある。

  • tf2::Transform
    変換や逆行列の計算に便利な型
  • tf2::StampedTransform
    tf2::Transformにタイムスタンプがついた型
  • geometry_msgs::Transform
    ROSトピックでよく使われる型
  • geometry_msgs::TransformStamped
    上記にタイムスタンプがついた型

TFの型まとめ

TFとしてbroadcastするにはタイムスタンプ型でなければならないが、tf2にはStampedTransform型が無いので注意。

クラスタイムスタンプなしタイムスタンプ付き内部で保持している値
tfTransformStampedTransform回転行列(3×3)+並進ベクトル(3×1)
tf2Transformなし(tf2::Stamped<Transform>はあるが、child_frameを記述できない)回転行列(3×3)+並進ベクトル(3×1)
geometry_msgsTransformTransformStampedクォータニオン(x,y,z,w)+並進ベクトル(x,y,z)

準備(CMakeLists.txt, package.xml, ヘッダファイル)

ROSで使うため、CMakeLists.txt, package.xmlに以下の記述が必要。

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  ︙
  tf2
  tf2_ros
  tf2_geometry_msgs
)

︙

catkin_package(
  INCLUDE_DIRS
  LIBRARIES tf_test
  CATKIN_DEPENDS roscpp ... tf2 tf2_ros tf2_geometry_msgs
  #DEPENDS system_lib
)

package.xml

<package format="2">
  <name>tf_test</name>
   ︙
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>

  <depend>roscpp</depend>
  <depend>rospy</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

ヘッダファイル

大体以下があれば大丈夫なはず。

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

TF Broadcaster

TFクラスとTF2クラスのbroadcasterの違い

TFを発信する際はタイムスタンプ付きの型(geometry_msgs::TransformStamped等)を使う。各クラスのBroadcasterが発信できる型は以下の通り。

tf::StampedTransformgeometry_msgs::TransformStamped
tf::broadcaster
tf2::broadcaster×
broadcastできる型の一覧

現在はtfクラスを使うことは少ないと思われるので、以降はtf2を例に説明します。

基本のbroadcaster

tf2::broadcasterはgeometry_msgs::TransformStamped型しか発信できないため、tf2::Transform型等を発信したければ一度geometry_msgs::TransformStampedに変換する必要がある。

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

void pub_tf(const geometry_msgs::TransformStamped trans_msg){
  // geometry_msgs::TransformStamped型のTFを発信
  static tf2_ros::TransformBroadcaster br;

  br.sendTransform(trans_msg);
}

void pub_tf(const tf2::Transform trans_tf2){
  // tf2::Transform型のTFを発信
  // tf2::Transform型は姿勢しか持っていないので、タイムスタンプやフレーム名を追加する必要がある
  static tf2_ros::TransformBroadcaster br;

  geometry_msgs::TransformStamped trans_msg;
  trans_msg.header.stamp = ros::Time::now();
  trans_msg.header.frame_id = "map";
  trans_msg.child_frame_id = "robot";
  trans_msg.transform = tf2::toMsg(trans_tf2);
  br.sendTransform(trans_msg);
}

なお、関数の最初で

static tf2_ros::TransformBroadcaster br;

という部分があるが、恥ずかしながらstaticを付ける効果がわからなかったので調べた。どうやらstaticを付けた変数は関数の実行時に毎回初期化されるのではなく、最初の一回だけ初期化された後は関数を抜けても残り続けるらしい。高速化のためなのか必ず必要なのかわからないが、つけておいたほうが良いと思う。

複数のTFを一度に発信する

発信できる型ならば、std::vectorで複数をまとめて同時に発信することも可能。

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

void publish_all(){
  // tf2でgeometry_msgsのTFを複数まとめて送信する例
  static tf2_ros::TransformBroadcaster br;

  geometry_msgs::TransformStamped tf1, tf2, tf3;
  std::vector<geometry_msgs::TransformStamped> all_tf;

  all_tf.push_back(tf1);
  all_tf.push_back(tf2);
  all_tf.push_back(tf3);

  br.sendTransform(all_tf);
}

TF listener

TFは現在の値を取得することはできない(ツリー状につながったTFがそれぞれ一定周期で更新されるため、現時点よりもちょっと古い情報しか持っていない)ため、二通りの方法がある。

下記のサイトがとても参考になった。

tf,tf2完全理解

方法① すべてのTFが更新されるまで待ってから取得する(基本)

現時点でのparentからchildまでのTFを取るために、その間のすべてのTFが更新されるのを待って取得する。この方法ではTFの時間的な補間がされるため正確なTFが取得できるが、現時点から少し経ってからしか結果が得られない。

bool get_tf(geometry_msgs::TransformStamped& tf, const std::string parent_frame, const std::string child_frame, const float timeout){
  // ここは良くない。tfBufferとtfListenerは関数の外で宣言し、できるだけ生存期間を長く取るほうが良い。(tfを常にlistenするため)
  static tf2_ros::Buffer tfBuffer;
  static tf2_ros::TransformListener tfListener(tfBuffer);

  // 時刻を記録する
  ros::Time now = ros::Time::now();

  // すべてのTFが補間可能になるまで待つ
  if(!tfBuffer.canTransform(parent_frame, child_frame, now, ros::Duration(timeout))){
    ROS_WARN("Could not lookup transform from %s to %s, in duration %f [sec]", parent_frame.c_str(), child_frame.c_str(), timeout);
    return false;
  }

  // nowの時刻でのtransformを取得する
  try{
    tf = tfBuffer.lookupTransform(parent_frame, child_frame, now, ros::Duration(0.1));
    return true;
  }
  catch(tf2::TransformException &ex){
    ROS_WARN("%s", ex.what());
    return false;
  }
}

方法② とにかく今のTFを取得する

とにかく今登録されている情報を使ってparentからchildまでのTFを取得する。この方法では即座に結果が帰ってくるが、parentからchild間に存在するそれぞれのTFが更新されたタイミングはバラバラなため、時間的にずれたTFをつなぐことになり正確ではない。

bool get_tf_immediate(geometry_msgs::TransformStamped& tf, const std::string parent_frame, const std::string child_frame){
  // ここは良くない。tfBufferとtfListenerは関数の外で宣言し、できるだけ生存期間を長く取るほうが良い。(tfを常にlistenするため)
  static tf2_ros::Buffer tfBuffer;
  static tf2_ros::TransformListener tfListener(tfBuffer);

  // ros::Time(0)=parentとchildの最新TFが登録された時刻のうち早い方
  // とりあえず今登録されているTFを取得するが、古い情報を使うため現在のTFとは異なる
  try{
    tf = tfBuffer.lookupTransform(parent_frame, child_frame, ros::Time(0), ros::Duration(0.1));
    return true;
  }
  catch(tf2::TransformException &ex){
    ROS_WARN("%s", ex.what());
    return false;
  }
}

TFの型変換

tf以外のもの→TF型

tf::Transformtf2::Transformgeometry_msgs::Transform
xyzとロール、ピッチ、ヨー
xyzとクォータニオン
変換行列(4×4)
TF型以外からTF型を作る場合

TF型→他のTF型、逆方向、合成等

tf::Transformtf2::Transformgeometry_msgs::Transform
xyzとロール、ピッチ、ヨー
xyzとクォータニオン
変換行列(4×4)
tf::Transform
tf2::Transform
geometry_msgs
逆方向のTF(なし)
2つのTFの合成(なし)
2つのTFの差分(なし)
TF型を変換したり、データを取り出す場合

xyzとロール、ピッチ、ヨーからgeometry_msgs

ロール、ピッチ、ヨーから直接geometry_msgs::Quaternionを作る関数は無いので、tf::Quaternionで作ってから代入する。

geometry_msgs::Transform xyz_rpy_to_geometry(double x, double y, double z, double roll, double pitch, double yaw){
  geometry_msgs::Transform gt;
  gt.translation.x = x;
  gt.translation.y = y;
  gt.translation.z = z;
  tf2::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  gt.rotation.x = q.x();
  gt.rotation.y = q.y();
  gt.rotation.z = q.z();
  gt.rotation.w = q.w();

  return gt;
}

xyzとクォータニオンからgeometry_msgs

xyzとクォータニオン(x,y,z,w)が分かっている場合、geometry_msgs::Transformにそのまま代入するだけ。

geometry_msgs::Transform xyz_quat_to_geometry(double x, double y, double z, double qx, double qy, double qz, double qw){
  geometry_msgs::Transform gt;
  gt.translation.x = x;
  gt.translation.y = y;
  gt.translation.z = z;
  gt.rotation.x = qx;
  gt.rotation.y = qy;
  gt.rotation.z = qz;
  gt.rotation.w = qw;

  return gt;
}

変換行列からgeometry_msgs

あまり使うことは無いかもしれないが、変換行列を並進ベクトル(3次元ベクトル)と回転行列(3×3行列)で表していた場合、geometry_msgsを作るにはこうする。

  geometry_msgs::Transform matrix_to_geometry(tf2::Matrix3x3 rot, tf2::Vector3 trans){
  geometry_msgs::Transform gt;
  gt.translation.x = trans.getX();
  gt.translation.y = trans.getY();
  gt.translation.z = trans.getZ();
  tf2::Quaternion q;
  rot.getRotation(q);
  gt.rotation.x = q.x();
  gt.rotation.y = q.y();
  gt.rotation.z = q.z();
  gt.rotation.w = q.w();

  return gt;
}

geometry_msgsからxyzとロール、ピッチ、ヨー

geometry_msgsからロール、ピッチ、ヨーを直接得ることはできないので、geometry_msgs::Quaternion→tf::Quaternion→tf::Matrix3x3に変換してからgetRPY関数で取得する。

std::tuple<double, double, double, double, double, double> geometry_to_xyz_rpy(geometry_msgs::Transform gt){
  double x = gt.translation.x;
  double y = gt.translation.y;
  double z = gt.translation.z;

  double roll, pitch, yaw;
  tf::Quaternion q(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w);
  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);

}

geometry_msgsからxyzとクォータニオン

geometry_msgsはxyzとクォータニオンを持つ型なので、そのまま取得する。

std::tuple<double, double, double, double, double, double, double> geometry_to_xyz_quat(geometry_msgs::Transform gt){
  double x = gt.translation.x;
  double y = gt.translation.y;
  double z = gt.translation.z;
  double qx = gt.rotation.x;
  double qy = gt.rotation.y;
  double qz = gt.rotation.z;
  double qw = gt.rotation.w;

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

geometry_msgsから変換行列

geometry_msgsから並進ベクトル(3次元)と回転行列(3×3行列)に変換する。

std::tuple<tf2::Matrix3x3, tf2::Vector3> geometry_to_matrix(geometry_msgs::Transform gt){
  tf2::Vector3 trans(gt.translation.x, gt.translation.y, gt.translation.z);
  tf2::Matrix3x3 rot(tf2::Quaternion(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w));

  return std::make_tuple(rot, trans);
}

geometry_msgsからtf::Transform

tf::Transform geometry_to_tf(geometry_msgs::Transform gt){
  tf::Vector3 trans(gt.translation.x, gt.translation.y, gt.translation.z);
  tf::Quaternion quat(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w);
  tf::Transform transform(quat, trans);

  return transform;
}

geometry_msgsからtf2::Transform

tf2では便利なconvert関数が使えるので、シンプルに変換できる。

tf2::Transform geometry_to_tf2(geometry_msgs::Transform gt){
  tf2::Transform tf2;
  tf2::convert(gt, tf2); // geometry_msgs::Transform -> tf2::Transform

  return tf2;
}

geometry_msgsの逆方向TF

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

geometry_msgsの合成

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

geometry_msgsの差分

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

xyzとロール、ピッチ、ヨーからtf2::Transform

コンストラクタを使ってtf2::Quaternion q(roll, pitch, yaw)とせず、一旦tf2::Quaternion q;で作ってからsetRPY関数で設定していることに注意。

tf2::Transform xyz_rpy_to_tf2(double x, double y, double z, double roll, double pitch, double yaw){
  tf2::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  tf2::Transform tf2(q, tf2::Vector3(x, y, z));

  return tf2;
}

xyzとクォータニオンからtf2::Transform

クォータニオンから作るときはtf2::Quaternionのコンストラクタを使える。

tf2::Transform xyz_quat_to_tf2(double x, double y, double z, double qx, double qy, double qz, double qw){
  tf2::Transform tf2(tf2::Quaternion(qx, qy, qz, qw), tf2::Vector3(x, y, z));
  return tf2;
}

変換行列からtf2::Transform

これは簡単。

tf2::Transform matrix_to_tf2(tf2::Matrix3x3 rot, tf2::Vector3 trans){
  tf2::Transform tf2(rot, trans);
  return tf2;
}

tf2::Transformからxyzとロール、ピッチ、ヨー

一旦tf::Matrix3x3を作って、getRPYで取り出すしか無いっぽい。tf2::Quaternionから直接作る等、もっとシンプルなり方をご存知の方がいましたらコメントで教えて下さい。

std::tuple<double, double, double, double, double, double> tf2_to_xyz_rpy(tf2::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();

  double roll, pitch, yaw;
  tf2::Quaternion q = tf2.getRotation();
  tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);
}

tf2::Transformからxyzとクォータニオン

クォータニオンの場合はシンプル。

  std::tuple<double, double, double, double, double, double, double> tf2_to_xyz_quat(tf2::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();

  double qx = tf2.getRotation().x();
  double qy = tf2.getRotation().y();
  double qz = tf2.getRotation().z();
  double qw = tf2.getRotation().w();

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

tf2::Transformから変換行列

とてもシンプル。

std::tuple<tf2::Matrix3x3, tf2::Vector3> tf2_to_matrix(tf2::Transform tf2){
  tf2::Matrix3x3 rot = tf2.getBasis();
  tf2::Vector3 trans = tf2.getOrigin();

  return std::make_tuple(rot, trans);
}

tf2::Transformからtf::Transform

ちょっと調べた限りでは変換する関数は見つからなかった。用途も特に無いかもしれない。どうしてもやるなら、一回数値型に変えてから作れば可能。

tf::Transform tf2_to_tf(tf::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();
  double qx = tf2.getRotation().x();
  double qy = tf2.getRotation().y();
  double qz = tf2.getRotation().z();
  double qw = tf2.getRotation().w();

  tf::Transform t(tf::Quaternion(qx, qy, qz, qw), tf::Vector3(x, y, z));
  return t;
}

tf2::Transformからgeometry_msgs

とてもシンプル。

geometry_msgs::Transform tf2_to_geometry_msgs(tf2::Transform tf2){
  geometry_msgs::Transform gt = tf2::toMsg(tf2);
  return gt;
}

tf2::Transformの逆方向TF

TF2使っててよかった!

tf2::Transform tf2_invert(tf2::Transform tf2){
  return tf2.inverse();
}

tf2::Transformの合成

2つのTFを合成する場合はその順番に注意。2つのTF(A,B)をA→Bの順番でつなげるなら座標変換の計算はA*B。

tf2::Transform tf2_add(tf2::Transform tf2_A, tf2::Transform tf2_B){
  return tf2_A* tf2_B;
}

tf2::Transformの差分

A,Bという2つのTFがあったとき、AとBの差分は以下のように計算できる。

tf2::Transform tf2_sub(tf2::Transform tf2_A, tf2::Transform tf2_B){
  return tf2_B * tf2_A.inverse();
}

xyzとロール、ピッチ、ヨーからtf::Transform

tf2::Transformの場合とほぼ同様のやり方でできる。

tf::Transform xyz_rpy_to_tf(double x, double y, double z, double roll, double pitch, double yaw){
  tf::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  tf::Transform t(q, tf::Vector3(x, y, z));

  return t;
}

xyzとクォータニオンからtf::Transform

クォータニオンから作るときはtf::Quaternionのコンストラクタを使える。

tf::Transform xyz_quat_to_tf(double x, double y, double z, double qx, double qy, double qz, double qw){
  tf::Transform t(tf::Quaternion(qx, qy, qz, qw), tf::Vector3(x, y, z));
  return t;
}

変換行列からtf2::Transform

これは簡単。

tf::Transform matrix_to_tf(tf::Matrix3x3 rot, tf::Vector3 trans){
  tf::Transform t(rot, trans);
  return t;
}

tf::Transformからxyzとロール、ピッチ、ヨー

一旦tf::Matrix3x3を作って、getRPYで取り出すしか無いっぽい。tf::Quaternionから直接作る等、もっとシンプルなり方をご存知の方がいましたらコメントで教えて下さい。

std::tuple<double, double, double, double, double, double> tf_to_xyz_rpy(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();

  double roll, pitch, yaw;
  tf::Quaternion q = t.getRotation();
  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);
}

tf::Transformからxyzとクォータニオン

クォータニオンの場合はシンプル。

std::tuple<double, double, double, double, double, double, double> tf_to_xyz_quat(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();

  double qx = t.getRotation().x();
  double qy = t.getRotation().y();
  double qz = t.getRotation().z();
  double qw = t.getRotation().w();

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

tf::Transformから変換行列

とてもシンプル。

std::tuple<tf::Matrix3x3, tf::Vector3> tf_to_matrix(tf::Transform t){
  tf::Matrix3x3 rot = t.getBasis();
  tf::Vector3 trans = t.getOrigin();

  return std::make_tuple(rot, trans);
}

tf::Transformからtf2::Transform

ちょっと調べた限りでは変換する関数は見つからなかった。用途も特に無いかもしれない。どうしてもやるなら、一回数値型に変えてから作れば可能。

tf2::Transform tf_to_tf2(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();
  double qx = t.getRotation().x();
  double qy = t.getRotation().y();
  double qz = t.getRotation().z();
  double qw = t.getRotation().w();

  tf2::Transform tf2(tf2::Quaternion(qx, qy, qz, qw), tf2::Vector3(x, y, z));
  return tf2;
}

tf::Transformからgeometry_msgs

tf2の時と関数名が違うので注意。こういう微妙な違いがあったりするのが怖い。

geometry_msgs::Transform tf_to_geometry_msgs(tf::Transform t){
  geometry_msgs::Transform gt;
  tf::transformTFToMsg(t, gt);
  return gt;
}

tf::Transformの逆方向TF

tf::Transform tf_invert(tf::Transform t){
  return t.inverse();
}

tf::Transformの合成

2つのTFを合成する場合はその順番に注意。2つのTF(A,B)をA→Bの順番でつなげるなら座標変換の計算はA*B。

tf::Transform tf_add(tf::Transform t_A, tf::Transform t_B){
  return t_A * t_B;
}

tf::Transformの差分

A,Bという2つのTFがあったとき、AとBの差分は以下のように計算できる。

tf::Transform tf_sub(tf::Transform t_A, tf::Transform t_B){
  return t_B * t_A.inverse();
}

▲準備 ▲broadcaster ▲listener ▲変換一覧

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

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

正しい固定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の勉強…