动态坐标变换(待续

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

C++:

1.1发布方：

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

/*
    发布方：需要订阅乌龟的位姿信息，转换成相当于窗体的坐标关系，并发布
    准 备：
        话题：/turtle1/pose
        消息: /turtle1/Pose

    流程：
    1.包含头文件
    2.设置编码、初始化、NodeHandle
    3.创建订阅对象，订阅/turtle1/pose
    4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系，并发布（关注）
    5.spin()
*/
void doPose(const turtlesim::Pose::ConstPtr& pose){
    //获取位姿信息，转换成坐标系相对关系（核心），并发布
    //a.创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    //b.组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    //坐标偏移量设置
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数
    /*
        位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是 2D ，没有翻滚与俯仰角度，所以可以得出乌龟的欧拉角：0 0 theta
    */
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);

    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    //c.发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    // 2.设置编码、初始化、NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    // 3.创建订阅对象，订阅/turtle1/pose
    ros::Subscriber sub = nh.subscribe("turtle1/pose",100,doPose);
    // 4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系，并发布（关注）
    
    // 5.spin()
    ros::spin();
    return 0;
}

调用rviz比较直观

1.2 订阅方

与静态坐标系一样，修改几个参数就可
1.参考的坐标系
2.坐标点的时间戳
3.目标坐标系

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

/*
    订阅方：订阅发布的坐标系相对关系，传入一个坐标点，调用 tf 实现转换
    流程：
        1.包含头文件
        2.设置编码、节点初始化、NodeHandle(必须的)
        3.创建一个订阅对象  --->订阅坐标系相对关系
        4.组织一个坐标点数据
        5.转换算法，调用 tf 内置实现
        6.最后输出
*/

int main(int argc, char *argv[])
{
    // 2.设置编码、节点初始化、NodeHandle(必须的)
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 3.创建一个订阅对象  --->订阅坐标系相对关系 (3-1 与 3-2 是结合使用的)
    // 3-1. 创建一个 buffer 缓存
    tf2_ros::Buffer buffer;
    // 3-2. 再创建一个监听对象(监听对象可以将订阅的数据存入 buffer )
    tf2_ros::TransformListener listen(buffer);
    // 4.组织一个坐标点数据
    geometry_msgs::PointStamped ps;
    //参考的坐标系
    ps.header.frame_id = "turtle1";
    //时间戳
    ps.header.stamp = ros::Time(0.0);
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    //添加休眠  （文章“注”的部分添加休眠）
    // ros::Duration(2).sleep();
    // 5.转换算法，调用 tf 内置实现
    ros::Rate rate(10);
    while (ros::ok())
    {
        // 核心代码 ---- 将 ps 转换为 base_link 的坐标点
        geometry_msgs::PointStamped ps_out;     
        /*
            调用了 buffer 的转换函数 transform 
            参数1：被转换的坐标点数据；
            参数2：目标坐标系；
            返回值：转换后的坐标点信息（输出的坐标系）

            注意：
            1.调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
            2.运行时存在的问题：抛出一个异常 base_link 不存在
                原因：订阅数据是一个耗时操作，可能在调用 transform 转换函数时，坐标系的相对关系还没有被订阅到，故出现异常
                解决：
                    方案1：在调用转换函数前执行休眠（简单）
                    方案2：进行异常处理
        */   

       try
       {
            ps_out = buffer.transform(ps,"world");  
            // 6.最后输出
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                        ps_out.point.x,
                        ps_out.point.y,
                        ps_out.point.z,
                        ps_out.header.frame_id.c_str());
       }
       catch(const std::exception& e)
       {
        //    std::cerr << e.what() << '\n';
           ROS_INFO("异常消息:%s",e.what());
       }
       
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

当启动键盘控制节点后，输出的坐标值会变

Python：

发布方：

#! usr/bin/env python
import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf_conversions

"""
    发布方：订阅乌龟的位姿信息，转换成坐标系的相对关系，再发布
    准  备：
        话题：/turtle1/pose
        类型：/turtlesim/Pose
    流  程：
        1.导包
        2.初始ROS化节点
        3.创建订阅对象
        4.回调函数处理订阅到的消息（核心）
        5.spin()
"""
def doPose(pose):
    #1. 创建一个发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    #2. 将 pose 转换成 坐标系相对关系消息
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()
    ts.child_frame_id = "turtle1"
    #子级坐标系相当于父级坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0.0
    #四元数
    #从 欧拉角 转换 四元数
    """
        乌龟是 2D 的,不存在x上的翻滚,y上的俯仰,只有z上的偏航
        0 0 pose.theta
    """
    qtn = tf_conversions.Transformations.quaternion_from_euler(0,0,pose.theta)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]
    #3. 发布
    pub.sendTransform(ts)

if __name__ == "__main__":
    # 2.初始ROS化节点
    rospy.init_node("dynamic_pub_p")
    # 3.创建订阅对象
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)
    # 4.回调函数处理订阅到的消息（核心）
    # 5.spin()
    rospy.spin()

订阅方：

与静态订阅方一样，修改部分内容（3个）
第四部分：时间戳、frame_id
try内：坐标系world

#! /usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs

"""
    订阅方:订阅坐标变换消息,传入被转换的坐标点,调用转换算法
    流程:
        1.导包
        2.初始化节点
        3.创建订阅对象
        4.组织被转换的坐标点
        5.转换逻辑实现,调用tf封装的算法
        6.输出结果
"""

if __name__ == "__main__":
    # 2.初始化节点
    rospy.init_node("static_sub_p")
    # 3.创建订阅对象
    # 3-1.创建缓存对象
    buffer = tf2_ros.Buffer()
    # 3-2.创建订阅对象的坐标点
    sub = tf2_ros.TransformListener(buffer)
    # 4.组织被转换的坐标点
    ps = tf2_geometry_msgs.PointStamped()
    # 时间戳 -- 0
    ps.header.stamp =rospy.Time()
    # 坐标系
    ps.header.frame_id = "turtle1"
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0
    # 5.转换逻辑实现,调用tf封装的算法
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            # 转换实现
            """"
                参数1:被转换的坐标点
                参数2:目标坐标系
                返回值:转换后的坐标点

                注:
                问题:抛出异常 base_link 不存在
                原因:转换函数调用时,可能还没有订阅到坐标系的相对信息
                解决: try 捕获异常,并处理
            """
            ps_out = buffer.transform(ps,"world")
            # 6.输出结果
            rospy.loginfo("转换后的坐标:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                            ps_out.point.x,
                            ps_out.point.y,
                            ps_out.point.z,
                            ps_out.header.frame_id
                            )
        except Exception as e:
            rospy.logwarn("错误提示:%s",e)
        rate.sleep()
 

备注：python未跑出来，python模块出错