ROSのTFへOdometoryを入力する

目的

ROSのgmappingなどでSLAMを行うためには、
TFで、"odom"→"base_link(or base_footprint)"の発行を行う必要がある。

ROSのLidarSLAMまとめ

実装

TFで、"odom"→"base_link(or base_footprint)"の発行を行うソースは以下

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

void odom_calc(double &state_odom_x,double &state_odom_y,double &state_odom_th){
    //ここでエンコーダ値を読み取り、オドメトリ変換する演算を行う。
}

int main(int argc, char **argv){
    tf::TransformBroadcaster odom_broadcaster;
    ros::Time current_time;
    current_time = ros::Time::now();

    ros::Rate r(30.0);

    double state_odom_x=0.0;//オドメトリX座標[m]
    double state_odom_y=0.0;//オドメトリY座標[m]
    double state_odom_th=0.0; //オドメトリ姿勢[rad]

    while(nh.ok()){
    odom_calc(state_odom_x,state_odom_y,state_odom_th);
    current_time = ros::Time::now();

    //tf odom->base_link
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = state_odom_x;
    odom_trans.transform.translation.y = state_odom_y;
    odom_trans.transform.translation.z = 0.0;
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(state_odom_th);
    odom_trans.transform.rotation = odom_quat;
    odom_broadcaster.sendTransform(odom_trans);

    ros::spinOnce();
    r.sleep();
    }
}

確認

rvizでtfが発行されていることを確認する。
座標系があっているかをよく確認する。

参考：

ROS wiki Publishing Odometry Information over ROS
http://wiki.ros.org/ja/navigation/Tutorials/RobotSetup/Odom