Rtimulib_rosでオイラー角出力

前提・実現したいこと

MPU9250リンク内容というセンサを使っていて
ros上でオイラー角(roll pitch yaw)を出力するために
Rtimulib_ros というプログラムを使っています(gitにあり)　

発生している問題・エラーメッセージ

rosに出力することができましたが　
・クウォータニオン
・加速度
・角速度
が出て　オイラー角が出ませんでした。
オイラー角に変換して出力しようにもうまくできません
Rtimulib2ディレクトリ内のRTMath.hをいじることで変わりそうですが・・　やり方がわかりません

該当のソースコード

リンク内容ノード等
リンク内容Rtimulibライブラリ

補足情報

Rtimulib_rosはRtimulib2というデモライブラリをros対応にしたものだと思います。
Rtimulib2ではセンサをデモ表示することができました。
Raspberry Pi 3Bを使っていてOSはUbuntu MATEです。
ディスプレイに表示させています。
ここに関してもC言語に関しても初心者なもので
足りない情報があれば追加していきます。もしよろしければよろしくお願いします。

行動規範の内容に同意します

回答1件

ベストアンサー

クォータニオンが表示されるというのは、rtimulib_ros.cpp のサンプルコードを実行した際の話を言っているのですよね？
コードを見ると、sensor_msgs::Imu メッセージオブジェクトに入れている以下のコードがあります。

imu_msg.orientation.x = imu_data.fusionQPose.x(); 
imu_msg.orientation.y = imu_data.fusionQPose.y(); 
imu_msg.orientation.z = imu_data.fusionQPose.z(); 
imu_msg.orientation.w = imu_data.fusionQPose.scalar();

この imu_msg.orientation は ROS でクォータニオンを表す geometry_msgs/Quaternion オブジェクトです。

これは tf パッケージを使うとオイラー角に変換できます。

imu_msg.orientation.x = imu_data.fusionQPose.x(); 
imu_msg.orientation.y = imu_data.fusionQPose.y(); 
imu_msg.orientation.z = imu_data.fusionQPose.z(); 
imu_msg.orientation.w = imu_data.fusionQPose.scalar(); 

// クォータニオンから RPY に変換する。
double roll, pitch, yaw;
tf::Matrix3x3(imu_msg.orientation).getRPY(roll, pitch, yaw);