ros中tf发布和程序接收

头文件：

#include <iostream>
#include <ros/package.h>
#include "Detect.h"
#include "utils.h"
#include <vector>
#include <map>
#include <thread>
#include <future>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/opencv.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/CameraInfo.h>
#include <pcl/common/common.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/registration/icp.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <pcl_ros/point_cloud.h>

发布：

rosrun tf static_transform_publisher 0.10904592 1.7957764 -0.71787471 -2.02833623 -1.4860462 -2.66345474 a base_link 100

注意：上面发布的tf代表a*R+T=b

对a来说是 xyz yaw pitch row

在目前用到的程序里，上面的发布是反的(opencv出来的结果)，一般的发布是：

rosrun tf static_transform_publisher 0.855750 0.115309 1.733910 -1.646041 -1.608207 -0.022006 base_link_1 a1 100

即b*R+T=a

对b来说是对a来说是 xyz yaw pitch row

 

接收：

声明：

        tf::TransformListener* tfl_;
        Eigen::Quaterniond rotation_final_;
        ::Eigen::Vector3d translation_final_;

定义：

                tfl_ = new tf::TransformListener();
                translation_final_   = Eigen::Vector3d(0,0,0);
                rotation_final_      = Eigen::Quaterniond::Identity();

	    try
            {
                if(tfl_)
                {
                tf::StampedTransform transform;
                //tfl_->waitForTransform("/base_link", msg_image->header.frame_id, ros::Time(0), ros::Duration(1.0));
                tfl_->lookupTransform("base_link_1","a1",ros::Time(0), transform);

                double roll, yaw, pitch;
                auto q = transform.getRotation();
                tf::Matrix3x3 m(q);
                m.getRPY(roll,pitch,yaw);

                ROS_INFO("TF: %f %f %f %f %f %f", transform.getOrigin().x(), 
                            transform.getOrigin().y(), 
                            transform.getOrigin().z(),
                            roll, yaw, pitch);

                translation_final_.x() = transform.getOrigin().x();
                translation_final_.y() = transform.getOrigin().y();
                translation_final_.z() = transform.getOrigin().z();
                rotation_final_ = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
                                * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
                                * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
                delete tfl_;
                tfl_ = NULL;
                }
            }
                catch (tf::TransformException &e)
                {
                    ROS_ERROR("TF error: %s", e.what());
                    return;
                }