TF 坐标变换案例：乌龟跟随

需求：

        生成一只 turtle1 采用键盘控制

        生成一只 turtle2 跟随 turtlr1

实现：

        创建turtle1 启动键盘控制

        创建 turtle2

        编写发布方：首先要订阅 turtle1 和 turtle2 的位姿信息pose，将位姿信息转换成为 tf 位姿信息发布

        编写订阅方：订阅两只乌龟的 tf 位姿信息，并计算相对坐标关系，编写跟随算法，并发布给turtls2

C++实现：

launch文件配置

<launch>
    <!-- 启动乌龟GUI节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />


    <node pkg="turtle_fllow" type="new_turtle" name="turtle2" output="screen" />
    <!-- 启动两个乌龟相对与世界的坐标关系的发布 -->
        <!-- 基本实现思路:
            1.节点只编写一个
            2.这个节点需要启动俩次
            3.节点启动时动态传参 第一次启动传递0turtle1 第二次启动turtle2
         -->
    <node pkg="turtle_fllow" type="pub_turtles" name="pub1" args="turtle1" output="screen" />
    <node pkg="turtle_fllow" type="pub_turtles" name="pub2" args="turtle2" output="screen" />
    
    <!-- 订阅 turtle1 turtle2 相对于 world 坐标的信息,转换陈为给 turtle1 相对于 turtle2 的相对信息,
    再生成速度消息, 
    -->
    <node pkg="turtle_fllow" type="turtle_control" name="control" output="screen" />
    
</launch>

生成一个turtle2

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*
    需求：向服务器发送请求，生成一个新的乌龟

    话题： /spawn
    消息： Type: turtlesim/Spawn
    srv:float32 x
        float32 y
        float32 theta
        string name
        ---
        string name

*/

int main(int argc,char *argv[]){
    ros::init(argc,argv,"ergouzi");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.x = 4;
    spawn.request.y = 4;
    spawn.request.theta = 3.14;
    spawn.request.name = "turtle2";
    client.waitForExistence();
    bool flag = client.call(spawn);
    if(flag){
        ROS_INFO("name=%s",spawn.response.name.c_str());
    }
    else{
        ROS_INFO("call service faild");
    }
    return 0;
}

订阅两个乌龟的坐标信息 tf 转换后发布

#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

        乌龟消息: turtlesim/Pose

        乌龟消息内容:
            float32 x
            float32 y
            float32 theta
            float32 linear_velocity
            float32 angular_velocity

    流程:
        1.包含头文件
        2.设置编码,初始化 NodeHeadel
        3.创建订阅对象,订阅 /turtle1/pose
        4.回调函数处理订阅的消息,将位姿信息转换成为相对坐标关系并发布
        5.spin
*/
//变量声明
std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr &pos)
{
    //获取位姿信息,转换成相对坐标关系,并发布
    //1.创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    //2.组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    //关键点2:frame_id动态传入
    ts.child_frame_id = turtle_name;  
    //坐标系偏移量的设置
    ts.transform.translation.x = pos->x;
    ts.transform.translation.y = pos->y;
    ts.transform.translation.z = 0.0;
    //坐标系四元数
    tf2::Quaternion qtn; //创建四元数对象
    //向该对象设置欧拉角。这个对象可以将欧拉角设置为四元数
    qtn.setRPY(0.0,0.0,pos->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();
   
    //3.发布
     pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    //2.设置编码,初始化 NodeHeadel
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dnc");
    ros::NodeHandle nh;
    /*
        解析lanunch文件,通过args参数传入
    */
   if (argc != 2)
   {
       ROS_ERROR("传入一个参数");
       return 1;
   }else{
       turtle_name = argv[1];
   }
   

    
    //3.创建订阅对象,订阅 /turtle1/pose
    //关键点1.订阅的话题名称,turtle 或 turtle2 是动态传入的
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",10,doPose);
    //4.回调函数处理订阅的消息,将位姿信息转换成为相对坐标关系并发布
    //5.spin
    ros::spin();
    return 0;
}

将tf坐标订阅后计算相对关系，算法处理后发布给turtle2

#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"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
/*
    需求:
        1.换算出 turtle1 相对与 turtle2 的相对关系
        2.根据距离计算角速度和线速度并发布
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    ros::init(argc,argv,"tfsS");
    ros::NodeHandle nh;

    tf2_ros::Buffer buf;
    tf2_ros::TransformListener sub(buf);

    ros::Rate rate(2);
    //创建发布对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",100);
    while (ros::ok())
    {
        try
        {
            //计算 son1 与 son2 的相对关系
            geometry_msgs::TransformStamped son1toson2 = 
                    buf.lookupTransform("turtle2","turtle1",ros::Time(0));
            // ROS_INFO("turtle2 相对与 turtle1 的信息:父级:%s,子集:%s 偏移量:(%.2f,%.2f,%.2f)",
            //         son1toson2.header.frame_id.c_str(),//turtle2
            //         son1toson2.child_frame_id.c_str(),//turtle1
            //         son1toson2.transform.translation.x,
            //         son1toson2.transform.translation.y,
            //         son1toson2.transform.translation.z
            //         );
            //根据相对坐标,计算并组织速度消息
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(son1toson2.transform.translation.x,2)+
                                    pow(son1toson2.transform.translation.y,2));
            twist.angular.z = 1 * atan2(son1toson2.transform.translation.y,
                                    son1toson2.transform.translation.x);

            //发布
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("error %s",e.what());
        }
        
        
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

注意 ：更改CMkeList 文件

python实现类似于C++情况

launch：

<launch>
    <!-- 启动乌龟GUI节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />


    <node pkg="turtle_fllow" type="new_turtle_p.py" name="turtle2" output="screen" />
    <!-- 启动两个乌龟相对与世界的坐标关系的发布 -->
        <!-- 基本实现思路:
            1.节点只编写一个
            2.这个节点需要启动俩次
            3.节点启动时动态传参 第一次启动传递0turtle1 第二次启动turtle2
         -->
    <node pkg="turtle_fllow" type="pub_turtles_p.py" name="pub1" args="turtle1" output="screen" />
    <node pkg="turtle_fllow" type="pub_turtles_p.py" name="pub2" args="turtle2" output="screen" />
    
    <!-- 订阅 turtle1 turtle2 相对于 world 坐标的信息,转换陈为给 turtle1 相对于 turtle2 的相对信息,
    再生成速度消息, 
    -->
    <node pkg="turtle_fllow" type="turtle_control_p.py" name="control" output="screen" />
    
</launch>

#! usr/bin/env python
import rospy
from turtlesim.srv import Spawn,SpawnRequest,SetPenResponse

# /*
#     需求：向服务器发送请求，生成一个新的乌龟
#     话题： /spawn
#     消息： Type: turtlesim/Spawn
#     srv:float32 x
#         float32 y
#         float32 theta
#         string name
#         ---
#         string name

# */

if __name__ == "__main__":
    rospy.init_node("sangouzi")
    client = rospy.ServiceProxy("/spawn",Spawn)
    spawn = SpawnRequest()
    spawn.x = 1
    spawn.y = 1
    spawn.theta = 1.57
    spawn.name = 'turtle2'
    client.wait_for_service()
    try:
        responce = client.call(spawn)
        rospy.loginfo("name = {}".format(responce.name))
    except Exception as e:
        rospy.loginfo("error")

import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf_conversions
import sys
'''
    发布方:订阅乌龟的位姿信息,转换成坐标系的相对关系,再进行发布
    准备:
        乌龟话题:/turtle1/pose

        乌龟消息: turtlesim/Pose

        乌龟消息内容:
            float32 x
            float32 y
            float32 theta
            float32 linear_velocity
            float32 angular_velocity

    流程:
        1.导包
        2.初始化 
        3.创建订阅对象,订阅 /turtle1/pose
        4.回调函数处理订阅的消息,将位姿信息转换成为相对坐标关系并发布
        5.spin
'''
turtle_name = ""

def doPos(pos):
    # 创建一个发布对象
    pub = tf2_ros.TransformBroadcaster()
    # 将 pos 位姿信息转换成坐标关系
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()
    ts.child_frame_id = turtle_name
    ts.transform.translation.x = pos.x
    ts.transform.translation.y = pos.y
    ts.transform.translation.z = 0

    #四元数

    qnt = tf_conversions.transformations.quaternion_from_euler(0, 0, pos.theta)
    ts.transform.rotation.x = qnt[0]
    ts.transform.rotation.y = qnt[1]
    ts.transform.rotation.z = qnt[2]
    ts.transform.rotation.w = qnt[3]

    pub.sendTransform(ts)
    
    # 发布
   

if __name__ == "__main__":
    rospy.init_node("dncP")
    #解析传入的参数(文件全路径 传入的参数 节点名称 日志文件路径)
    if len(sys.argv) != 4:
        rospy.logerr("参数错误")
        sys.exit()
    else:
        turtle_name = sys.argv[1]
    
    sub  = rospy.Subscriber(turtle_name+"/pose",Pose,doPos)
    rospy.spin()
    
       

from os import tcsetpgrp
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from tf2_ros import buffer
from geometry_msgs.msg import Twist
import math
'''
    订阅方实现：订阅坐标消息，传入被转换的座标点，调用转换算法
    流程：
        1、导包
        2、初始化 ros 节点
        3、创建订阅对象
        4、组织被转换的坐标点
        5、转换逻辑编写，调用 tf 的算法
        6、输出结果
        7、spin
'''

if __name__ == "__main__":

    # 2、初始化 ros 节点
    rospy.init_node("staS")
    # 3、创建订阅对象
        #创建一个缓存对象
    buf = tf2_ros.Buffer()
        #创建订阅对象，将缓存传入
    sub = tf2_ros.TransformListener(buf)

    pub = rospy.Publisher("turtle2/cmd_vel",Twist,queue_size=100)
    # 5、转换逻辑编写，调用 tf 的算法
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            #计算 son1 相对 son2 的坐标关系
            son1toson2 = buf.lookup_transform("turtle2","turtle1",rospy.Time(0))
            # rospy.loginfo("父级:%s,子级:%s 坐标(%.2f,%.2f,%.2f)",
            #                 son1toson2.header.frame_id,
            #                 son1toson2.child_frame_id,
            #                 son1toson2.transform.translation.x,
            #                 son1toson2.transform.translation.y,
            #                 son1toson2.transform.translation.z)
            tws = Twist()
            tws.linear.x = 0.5 * math.sqrt(math.pow(son1toson2.transform.translation.x,2)+
                                            math.pow(son1toson2.transform.translation.y,2))
            tws.angular.z = 2 * math.atan2(son1toson2.transform.translation.y,
                                        son1toson2.transform.translation.x)
            pub.publish(tws)
        except Exception as e:

            rospy.logwarn("error:%s",e)
        rate.sleep()
    
    # 6、输出结果