IMUからのトピックをTFに変換する

6461 ワード
C++ RealSenseD435i ROS RealSense C++ テキストリンク

この記事は

ROSを使っている人向けの記事です。TFの知識が少しあると良いかもです。

使っているセンサー

Realsense D435i

Realsenseの導入方法

少し記事が古いですが、参考にどうぞ
RealSense D435のROSへの導入でつまづいたところ

コード

GithubでROSのパッケージも公開してます。(ライセンスとかは今度編集します…。)
shrimp-f/imu2tf

imu2tf_converter.cpp
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <sensor_msgs/Imu.h>


void poseCallback(const sensor_msgs::Imu& msg){
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
    tf::Quaternion q;
    double g = 10.0; //重力加速度
    double r,p; //それぞれroll,pitch。下は、それぞれトピックの値から計算
    r = atan(msg.linear_acceleration.y/msg.linear_acceleration.z);
    p = atan(-msg.linear_acceleration.x/(sqrt(pow(msg.linear_acceleration.y,2)+pow(msg.linear_acceleration.z,2))));
    q.setRPY(r, p, 0);
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "camera_link"));//親フレームをworld,子フレームをcamera_linkとしてTFを配信
}


int main(int argc, char** argv){
    ros::init(argc, argv, "my_tf_broadcaster");
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe("/camera/accel/sample", 10, &poseCallback);//ここでIMUのトピックを購読

    ros::spin();
    return 0;
};

最後に

トピック名とTFのフレームは適宜自分の使いたい名前に変えてください。
TFはちゃんと調べて理解するとROSの世界が広がる(気がする)ので頑張ってください！

pythonノートの使用
ニューラルネットワーク-スタート