ros数据转换

写在前面

1、本文内容
ros数据转换

2、平台
ubuntu1804, ros melodic
3、转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/127412292

转换

ros-pcl

https://blog.csdn.net/stephanezhang/article/details/122214518

ROS-Eigen消息转换

https://www.codenong.com/cs110367661/
https://blog.csdn.net/u012424737/article/details/110367661
http://docs.ros.org/en/jade/api/eigen_conversions/html/eigen__msg_8h.html

geometry_msgs::Transform转eigen matrix4x4

Eigen::Matrix4d transform_to_mat(geometry_msgs::Transform transform){
	Eigen::Matrix4d transform_mat = Eigen::Matrix4d::Identity();
	Eigen::Quaterniond quat_eigen = Eigen::Quaterniond::Identity();
	tf::quaternionMsgToEigen(transform.rotation, quat_eigen);
	transform_mat.block<3, 3>(0, 0) = quat_eigen.matrix();
	transform_mat(0, 3) = transform.translation.x;
	transform_mat(1, 3) = transform.translation.y;
	transform_mat(2, 3) = transform.translation.z;
	return transform_mat;
}

references://https://blog.csdn.net/HelloJinYe/article/details/106926187

geometry_msgs::Pose与Eigen::Isometry3d(Eigen::Matrix4d)互相转换

头文件

#include <eigen_conversions/eigen_msg.h>

代码

    geometry_msgs::Pose odom;
    Eigen::Isometry3d mat_Isometry3d;
    
    tf::poseMsgToEigen(odom, mat_Isometry3d);
    Eigen::Matrix4d matrix = mat_Isometry3d.matrix();

    tf::poseEigenToMsg(mat_Isometry3d, odom);

tf发布坐标系关系

假设将baselink下的点云转到map下的矩阵为mat_baselink2map，想要发布baselink和map的tf关系
1、launch中静态发布

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="baselink2map" args="0 0 0 0 0 0 1 map base_link" />
</launch>

其中"0 0 0 0 0 0 1 map base_link"：tx ty tz qx qy qz qw frame_id child_frame_id

2、代码发布

#include <iostream>

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

#include <eigen_conversions/eigen_msg.h>
#include <pcl/common/transforms.h>
#include <Eigen/Core>
#include <Eigen/Dense>

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "odom_baselink2map_node");
    ros::NodeHandle nh;

    ros::Publisher pub_odom = nh.advertise<nav_msgs::Odometry>("/odom_baselink2map", 100000);
    nav_msgs::Odometry odom_baselink2map;
    tf::TransformBroadcaster br;


    ros::Rate rate(5);
    double x = 0;
    double y = 0;
    while (ros::ok())
    {
        if (fabs(x - 0.99) > 1e-6)
        {
            x += 0.01;
            y = std::sqrt(1.0 - x * x);
        }else{
            x = 0;
        }

        Eigen::Matrix4d mat_baselink2map = Eigen::Matrix4d::Identity();
        mat_baselink2map << 1, 0, 0, x,
            0, 1, 0, y,
            0, 0, 1, 0,
            0, 0, 0, 1;
        Eigen::Isometry3d Isometry3d_baselink2map;
        Isometry3d_baselink2map.matrix() = mat_baselink2map;
        std::cout << "Isometry3d_baselink2map\n"
                  << Isometry3d_baselink2map.matrix() << std::endl;
        geometry_msgs::Pose pose_baselink2map;
        tf::poseEigenToMsg(Isometry3d_baselink2map, pose_baselink2map);
        odom_baselink2map.pose.pose= pose_baselink2map;
        odom_baselink2map.header.frame_id = "map";
        odom_baselink2map.child_frame_id = "base_link";
        odom_baselink2map.header.stamp = ros::Time::now();
        pub_odom.publish(odom_baselink2map);

        tf::StampedTransform transform;
        tf::Quaternion q;
        q.setX(pose_baselink2map.orientation.x);
        q.setY(pose_baselink2map.orientation.y);
        q.setZ(pose_baselink2map.orientation.z);
        q.setW(pose_baselink2map.orientation.w);
        transform.setRotation(q);
        transform.setOrigin(tf::Vector3(pose_baselink2map.position.x,
                                        pose_baselink2map.position.y,
                                        pose_baselink2map.position.z));
        transform.stamp_ = ros::Time::now();
        transform.frame_id_ = "map";
        transform.child_frame_id_ = "base_link";
        br.sendTransform(transform);

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

    return 0;
}

发布baselink到map的关系(baselink在map下的位置)，那么map是父(frame_id)，baselink是子(child_frame_id)

获取baselink到map的关系时，同样map是父，baselink是子

#include <iostream>

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

#include <eigen_conversions/eigen_msg.h>
#include <pcl/common/transforms.h>
#include <Eigen/Core>
#include <Eigen/Dense>



bool GetTfTransformToMatrix(std::string frame_id, std::string child_frame_id, Eigen::Matrix4d &matrix)
{

    // 获取pose
    tf::StampedTransform pose_;
    tf::TransformListener tf_listener_;
    try
    {
        tf_listener_.waitForTransform(frame_id, child_frame_id, ros::Time(0), ros::Duration(3));
        tf_listener_.lookupTransform(frame_id, child_frame_id, ros::Time(0), pose_);
    }
    catch (tf::TransformException &e)
    {
        ROS_ERROR("[GetTransformMatrix]: %s", e.what());
        return false;
    }

    Eigen::Vector3d translation = Eigen::Vector3d(pose_.getOrigin().x(), pose_.getOrigin().y(), pose_.getOrigin().z());
    Eigen::Quaterniond quat = Eigen::Quaterniond::Identity();

    quat = Eigen::Quaterniond(pose_.getRotation().w(),
                              pose_.getRotation().x(),
                              pose_.getRotation().y(),
                              pose_.getRotation().z());
    Eigen::Matrix3d rotation = quat.matrix();

    // Eigen::Matrix4d mat_pose = Eigen::Matrix4d::Identity();
    matrix.block<3, 3>(0, 0) = rotation;
    matrix.matrix().block<3, 1>(0, 3) = translation;
    return true;
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "odom_baselink2map_listen_node");
    ros::NodeHandle nh;


    ros::Rate rate(5);
    double x = 0;
    double y = 0;
    while (ros::ok())
    {
        Eigen::Matrix4d matrix = Eigen::Matrix4d::Identity();
        GetTfTransformToMatrix("map", "base_link", matrix);

        std::cout << "matrix\n"
                  << matrix << std::endl;

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

    return 0;
}

参考

文中已列出