ROSのTFからSLAMの結果を受信する。

目的

ROSのTFからSLAM(gmapping/cartographerなど)の自己位置推定結果を受信する。

ROSのLidarSLAMまとめ

実装

TFで、"map"→"base_link(or base_footprint)"を受信する。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <iostream>
using namespace std;

int main(int argc, char *argv[]){
    ros::init(argc, argv, "tf_check");
    ros::NodeHandle n;
    tf::TransformListener listener(ros::Duration(10));

    ros::NodeHandle private_nh("~");
    string tf_name1;
    string tf_name2;
    private_nh.param("tf_name1",tf_name1,std::string("/base_link"));
    private_nh.param("tf_name2",tf_name2,std::string("/map"));

    double x_m=0.0, y_m=0.0, th_m=0.0;

    tf::StampedTransform trans_slam;

    while (n.ok())  {
    try {
        listener.lookupTransform(tf_name2, tf_name1,ros::Time(0), trans_slam);
        x_m = trans_slam.getOrigin().x();
        y_m = trans_slam.getOrigin().y();
        th_m = tf::getYaw(trans_slam.getRotation());
    }
    catch (tf::TransformException &ex)  {
        ROS_ERROR("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }

    cout<< "SLAM result" << ",";
    cout<< x_m << ",";
    cout<< y_m << ",";
    cout<< th_m/M_PI*180.0<< endl;

    }

参考

MyEnigma:世界で一番簡単なtfの使い方
http://myenigma.hatenablog.com/entry/20130210/1360491625
tf,tf2完全理解,Koji Terada,第五回ROS勉強会 @名古屋
https://www.slideshare.net/kojiterada5/tftf2