ros之TF坐标转换

青旅半醒 2023-02-25 03:40 2阅读 0赞

##                                         ros之TF坐标转换 ##

# 1.c++ #

demo01\_turtle1.cpp

#include <iostream>
    #include <ros/ros.h>
    #include <turtlesim/Pose.h>
    #include <tf/transform_broadcaster.h>
    using namespace std;
    
    void callBack(const turtlesim::Pose::ConstPtr &pose,tf::TransformBroadcaster *broadcaster){
    //    ROS_INFO_STREAM("receive turtle1 pose");
        //发送的数据(位置和姿态)
        tf::Transform transform;
        //设置位置
        transform.setOrigin(tf::Vector3{pose->x,pose->y,0});
        //设置姿态 rx ry rz  rpy（欧拉角）
        //四元数 可以表示姿态 (欧拉角可以和四元数相互转换)
        //四元数
        tf::Quaternion quaternion;
        quaternion.setRPY(0,0,pose->theta);
        transform.setRotation(quaternion);
        //发送给tf
        broadcaster->sendTransform(tf::StampedTransform(transform,ros::Time().now(),"world","turtle1"));
    }
    
    int main(int argc,char *argv[]){
        //节点名
        string nodeName = "demo01_turtle1";
        //初始化节点
        ros::init(argc,argv,nodeName);
        //创建节点
        ros::NodeHandle node;
        /*-------------------------- 创建broadcast --------------------------*/
        tf::TransformBroadcaster broadcaster;
        /*-------------------------- 获取小乌龟1坐标 --------------------------*/
        ros::Subscriber subscriber = node.subscribe<turtlesim::Pose>("/turtle1/pose", 5, boost::bind(callBack,_1,&broadcaster));
    
        //事件轮询
        ros::spin();
        return 0;
    }

demo01\_turtle2.cpp

#include <iostream>
    #include <ros/ros.h>
    #include <turtlesim/Spawn.h>
    #include <turtlesim/Pose.h>
    #include <tf/transform_broadcaster.h>
    #include <tf/transform_listener.h>
    
    using namespace std;
    
    void callBack(const turtlesim::Pose::ConstPtr &pose, tf::TransformBroadcaster *broadcaster) {
    //    ROS_INFO_STREAM("receive turtle2 pose");
        //发送的数据(位置和姿态)
        tf::Transform transform;
        //设置位置
        transform.setOrigin(tf::Vector3{pose->x, pose->y, 0});
        //设置姿态 rx ry rz  rpy（欧拉角）
        //四元数 可以表示姿态 (欧拉角可以和四元数相互转换)
        //四元数
        tf::Quaternion quaternion;
        quaternion.setRPY(0, 0, pose->theta);
        transform.setRotation(quaternion);
        //发送给tf
        broadcaster->sendTransform(tf::StampedTransform(transform, ros::Time().now(), "world", "turtle2"));
    }
    
    int main(int argc, char *argv[]) {
        //节点名
        string nodeName = "demo01_turtle2";
        //初始化节点
        ros::init(argc, argv, nodeName);
        //创建节点
        ros::NodeHandle node;
        /*-------------------------- asy --------------------------*/
        ros::AsyncSpinner spinner(1);
        spinner.start();
        /*-------------------------- 创建小乌龟 --------------------------*/
        ros::ServiceClient client = node.serviceClient<turtlesim::Spawn>("/spawn");
        //等待服务
        client.waitForExistence();
        //数据
        turtlesim::Spawn spawn;
        spawn.request.x = 3.0;
        spawn.request.y = 3.0;
        spawn.request.theta = 0.0;
        spawn.request.name = "turtle2";
        //调用服务
        client.call(spawn);
    
        /*-------------------------- 创建broadcast --------------------------*/
        tf::TransformBroadcaster broadcaster;
        /*-------------------------- 获取小乌龟2坐标 --------------------------*/
        ros::Subscriber subscriber = node.subscribe<turtlesim::Pose>("/turtle2/pose", 5,
                                                                     boost::bind(callBack, _1, &broadcaster));
     ros::Publisher publisher = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 5);
        /*-------------------------- 获取小乌龟相对位置 --------------------------*/
        tf::TransformListener listener;
        while (ros::ok()){
            //接收结果
            tf::StampedTransform transform;
            try {
                //参数1：最终表达的坐标系
                //参数2：需要表示的坐标系
                //参数3：时间戳  传(0)最近时间
                //参数4：接收最终数据
                listener.lookupTransform("turtle2", "turtle1", ros::Time(0), transform);
                //获取坐标
                double x = transform.getOrigin().x();
                double y = transform.getOrigin().y();
                ROS_INFO_STREAM("x--------- "<<x<<"    y------- "<<y);
                tf::Quaternion quaternion = transform.getRotation();
                double z = quaternion.z();
                geometry_msgs::Twist twist;
                double distance = sqrt(pow(x, 2) + pow(y, 2));
                double angular = atan2(y, x);
                ///turtle1/cmd_vel 跟着乌龟１走
                twist.linear.x=distance*0.5;
                twist.angular.z=angular*3;
                publisher.publish(twist);
    
            } catch (exception e) {
    //            ROS_INFO_STREAM("exception");
            }
        }
    
        //等待程序停止
    //    ros::waitForShutdown();
        return 0;
    }

------------------------------------------------------------------------------------------------------------------------

demo02\_turtle1.cpp

//
    // Created by wt on 2020/7/9.
    //
    
    #include <iostream>
    #include <ros/ros.h>
    #include <turtlesim/Pose.h>
    #include <tf/transform_broadcaster.h>
    using namespace std;
    
    void sendTransform(double x,double y,double theta,string parent_frame,string child_frame,tf::TransformBroadcaster *broadcaster){
        //发送的数据(位置和姿态)
        tf::Transform transform;
        //设置位置
        transform.setOrigin(tf::Vector3{x,y,0});
        //设置姿态 rx ry rz  rpy（欧拉角）
        //四元数 可以表示姿态 (欧拉角可以和四元数相互转换)
        //四元数
        tf::Quaternion quaternion;
        quaternion.setRPY(0,0,theta);
        transform.setRotation(quaternion);
        //发送给tf
        broadcaster->sendTransform(tf::StampedTransform(transform,ros::Time().now(),parent_frame,child_frame));
    }
    
    void callBack(const turtlesim::Pose::ConstPtr &pose,tf::TransformBroadcaster *broadcaster){
    //    ROS_INFO_STREAM("receive turtle1 pose");
        //发布turtle1的坐标原点的位置
        sendTransform(pose->x,pose->y,pose->theta,"world","turtle1",broadcaster);
        //基于turtle1坐标系发布后面这个点
        sendTransform(-2,0,pose->theta,"turtle1","back",broadcaster);
        //上面的点
        sendTransform(0,2,pose->theta,"turtle1","up",broadcaster);
        //下面点
        sendTransform(0,-2,pose->theta,"turtle1","down",broadcaster);
    
    }
    
    int main(int argc,char *argv[]){
        //节点名
        string nodeName = "demo01_turtle1";
        //初始化节点
        ros::init(argc,argv,nodeName);
        //创建节点
        ros::NodeHandle node;
        /*-------------------------- 创建broadcast --------------------------*/
        tf::TransformBroadcaster broadcaster;
        /*-------------------------- 获取小乌龟1坐标 --------------------------*/
        ros::Subscriber subscriber = node.subscribe<turtlesim::Pose>("/turtle1/pose", 5, boost::bind(callBack,_1,&broadcaster));
    
        //事件轮询
        ros::spin();
        return 0;
    }

demo02\_turtle2.cpp

//
    // Created by wt on 2020/7/9.
    //
    #include <iostream>
    #include <ros/ros.h>
    #include <turtlesim/Spawn.h>
    #include <turtlesim/Pose.h>
    #include <tf/transform_broadcaster.h>
    #include <tf/transform_listener.h>
    #include <geometry_msgs/Twist.h>
    
    using namespace std;
    
    void callBack(const turtlesim::Pose::ConstPtr &pose, tf::TransformBroadcaster *broadcaster,string turtleName) {
    //    ROS_INFO_STREAM("receive turtle2 pose");
        //发送的数据(位置和姿态)
        tf::Transform transform;
        //设置位置
        transform.setOrigin(tf::Vector3{pose->x, pose->y, 0});
        //设置姿态 rx ry rz  rpy（欧拉角）
        //四元数 可以表示姿态 (欧拉角可以和四元数相互转换)
        //四元数
        tf::Quaternion quaternion;
        quaternion.setRPY(0, 0, pose->theta);
        transform.setRotation(quaternion);
        //发送给tf
        broadcaster->sendTransform(tf::StampedTransform(transform, ros::Time().now(), "world", turtleName));
    }
    
    int main(int argc, char *argv[]) {
        //乌龟名
        string turtleName = argv[1];
        //追随点的位置
        string position = argv[2];
    
        //节点名
        string nodeName = "demo01_turtle2";
        //初始化节点
        ros::init(argc, argv, nodeName);
        //创建节点
        ros::NodeHandle node;
        /*-------------------------- asy --------------------------*/
        ros::AsyncSpinner spinner(1);
        spinner.start();
        /*-------------------------- 创建小乌龟 --------------------------*/
        ros::ServiceClient client = node.serviceClient<turtlesim::Spawn>("/spawn");
        //等待服务
        client.waitForExistence();
        //数据
        turtlesim::Spawn spawn;
        spawn.request.x = 3.0;
        spawn.request.y = 3.0;
        spawn.request.theta = 0.0;
        spawn.request.name = turtleName;
        //调用服务
        client.call(spawn);
    
        /*-------------------------- 创建broadcast --------------------------*/
        tf::TransformBroadcaster broadcaster;
        /*-------------------------- 发布小乌龟2的移动 --------------------------*/
        //移动小乌龟2
        const ros::Publisher &publisher = node.advertise<geometry_msgs::Twist>("/"+turtleName+"/cmd_vel", 5);
        /*-------------------------- 获取小乌龟2坐标 --------------------------*/
        ros::Subscriber subscriber = node.subscribe<turtlesim::Pose>("/"+turtleName+"/pose", 5,
                                                                     boost::bind(callBack, _1, &broadcaster,turtleName));
        /*-------------------------- 获取小乌龟相对位置 --------------------------*/
        tf::TransformListener listener;
    //    ros::Rate rate(100);
        while (ros::ok()){
            //接收结果
            tf::StampedTransform transform;
            try {
                //参数1：最终表达的坐标系
                //参数2：需要表示的坐标系
                //参数3：时间戳  传(0)最近时间
                //参数4：接收最终数据
                listener.lookupTransform(turtleName, position, ros::Time(0), transform);
                //获取坐标
                double x = transform.getOrigin().x();
                double y = transform.getOrigin().y();
    
                //计算小乌龟2和小乌龟1的距离
                double distance = sqrt(pow(x,2)+pow(y,2));
                //计算小乌龟1和2的夹角
                double angle = atan2(y,x);
                //角度范围在0-180 -180-0
                if(angle>M_PI){
                    angle -= 2*M_PI;
                } else if (angle<-M_PI){
                    angle += 2*M_PI;
                }
                //数据
                geometry_msgs::Twist twist;
                twist.linear.x = distance*0.6;
                twist.angular.z = 6*angle;
                //发布数据
                publisher.publish(twist);
    //            rate.sleep();
            } catch (exception e) {
    //            ROS_INFO_STREAM("exception");
            }
        }
        //等待程序停止
    //    ros::waitForShutdown();
        return 0;
    }

\----------------------------------------------------------------------------------------------------------------------------------------------------

demo03\_turtle1.cpp

//
    // Created by wt on 2020/7/9.
    //
    
    #include <iostream>
    #include <ros/ros.h>
    #include <turtlesim/Pose.h>
    #include <tf/transform_broadcaster.h>
    using namespace std;
    
    //旋转角度
    double angle = 0;
    
    void sendTransform(double x,double y,double theta,string parent_frame,string child_frame,tf::TransformBroadcaster *broadcaster){
        //发送的数据(位置和姿态)
        tf::Transform transform;
        //设置位置
        transform.setOrigin(tf::Vector3{x,y,0});
        //设置姿态 rx ry rz  rpy（欧拉角）
        //四元数 可以表示姿态 (欧拉角可以和四元数相互转换)
        //四元数
        tf::Quaternion quaternion;
        quaternion.setRPY(0,0,theta);
        transform.setRotation(quaternion);
        //发送给tf
        broadcaster->sendTransform(tf::StampedTransform(transform,ros::Time().now(),parent_frame,child_frame));
    }
    
    void callBack(const turtlesim::Pose::ConstPtr &pose,tf::TransformBroadcaster *broadcaster){
    //    ROS_INFO_STREAM("receive turtle1 pose");
        //发布turtle1的坐标原点的位置
        sendTransform(pose->x,pose->y,pose->theta,"world","turtle1",broadcaster);
    
        //圆的半径
        double r = 2.0;
        //基于turgle1的坐标
        double x = r*cos(angle*M_PI/180);
        double y = r*sin(angle*M_PI/180);
        //基于turtle1坐标系发布圆上的点
        sendTransform(x,y,pose->theta,"turtle1","round",broadcaster);
        //修改angle
        ++angle;
    }
    
    int main(int argc,char *argv[]){
        //节点名
        string nodeName = "demo01_turtle1";
        //初始化节点
        ros::init(argc,argv,nodeName);
        //创建节点
        ros::NodeHandle node;
        /*-------------------------- 创建broadcast --------------------------*/
        tf::TransformBroadcaster broadcaster;
        /*-------------------------- 获取小乌龟1坐标 --------------------------*/
        ros::Subscriber subscriber = node.subscribe<turtlesim::Pose>("/turtle1/pose", 5, boost::bind(callBack,_1,&broadcaster));
    
        //事件轮询
        ros::spin();
        return 0;
    }

demo03\_turtle2.cpp

//
    // Created by wt on 2020/7/9.
    //
    #include <iostream>
    #include <ros/ros.h>
    #include <turtlesim/Spawn.h>
    #include <turtlesim/Pose.h>
    #include <tf/transform_broadcaster.h>
    #include <tf/transform_listener.h>
    #include <geometry_msgs/Twist.h>
    
    using namespace std;
    
    void callBack(const turtlesim::Pose::ConstPtr &pose, tf::TransformBroadcaster *broadcaster) {
    //    ROS_INFO_STREAM("receive turtle2 pose");
        //发送的数据(位置和姿态)
        tf::Transform transform;
        //设置位置
        transform.setOrigin(tf::Vector3{pose->x, pose->y, 0});
        //设置姿态 rx ry rz  rpy（欧拉角）
        //四元数 可以表示姿态 (欧拉角可以和四元数相互转换)
        //四元数
        tf::Quaternion quaternion;
        quaternion.setRPY(0, 0, pose->theta);
        transform.setRotation(quaternion);
        //发送给tf
        broadcaster->sendTransform(tf::StampedTransform(transform, ros::Time().now(), "world", "turtle2"));
    }
    
    int main(int argc, char *argv[]) {
        //节点名
        string nodeName = "demo01_turtle2";
        //初始化节点
        ros::init(argc, argv, nodeName);
        //创建节点
        ros::NodeHandle node;
        /*-------------------------- asy --------------------------*/
        ros::AsyncSpinner spinner(1);
        spinner.start();
        /*-------------------------- 创建小乌龟 --------------------------*/
        ros::ServiceClient client = node.serviceClient<turtlesim::Spawn>("/spawn");
        //等待服务
        client.waitForExistence();
        //数据
        turtlesim::Spawn spawn;
        spawn.request.x = 3.0;
        spawn.request.y = 3.0;
        spawn.request.theta = 0.0;
        spawn.request.name = "turtle2";
        //调用服务
        client.call(spawn);
    
        /*-------------------------- 创建broadcast --------------------------*/
        tf::TransformBroadcaster broadcaster;
        /*-------------------------- 发布小乌龟2的移动 --------------------------*/
        //移动小乌龟2
        const ros::Publisher &publisher = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 5);
        /*-------------------------- 获取小乌龟2坐标 --------------------------*/
        ros::Subscriber subscriber = node.subscribe<turtlesim::Pose>("/turtle2/pose", 5,
                                                                     boost::bind(callBack, _1, &broadcaster));
        /*-------------------------- 获取小乌龟相对位置 --------------------------*/
        tf::TransformListener listener;
    //    ros::Rate rate(100);
        while (ros::ok()){
            //接收结果
            tf::StampedTransform transform;
            try {
                //参数1：最终表达的坐标系
                //参数2：需要表示的坐标系
                //参数3：时间戳  传(0)最近时间
                //参数4：接收最终数据
                listener.lookupTransform("turtle2", "round", ros::Time(0), transform);
                //获取坐标
                double x = transform.getOrigin().x();
                double y = transform.getOrigin().y();
    
                //计算小乌龟2和小乌龟1的距离
                double distance = sqrt(pow(x,2)+pow(y,2));
                //计算小乌龟1和2的夹角
                double angle = atan2(y,x);
                //角度范围在0-180 -180-0
                if(angle>M_PI){
                    angle -= 2*M_PI;
                } else if (angle<-M_PI){
                    angle += 2*M_PI;
                }
                //数据
                geometry_msgs::Twist twist;
                twist.linear.x = distance*0.6;
                twist.angular.z = 6*angle;
                //发布数据
                publisher.publish(twist);
    //            rate.sleep();
            } catch (exception e) {
    //            ROS_INFO_STREAM("exception");
            }
        }
    
        //等待程序停止
    //    ros::waitForShutdown();
        return 0;
    }

### 2.python ###

demo01\_turtle1.py

#!/usr/bin/env python
    # coding:utf-8
    import rospy
    from turtlesim.msg import Pose
    from tf.broadcaster import TransformBroadcaster
    from tf.transformations import quaternion_from_euler
    
    def callBack(pose):
        # 坐标
        translation = (pose.x,pose.y,0)
        # 姿态 (0,0,pose.theta)
        rotation = quaternion_from_euler(0,0,pose.theta)
        # 发送给tf
        broadcaster.sendTransform(translation,rotation,rospy.Time().now(),'turtle1','world')
    
    if __name__ == '__main__':
        # 创建节点
        rospy.init_node("demo01_turtle1")
        #------------------------ tf发布者 ------------------------#
        broadcaster = TransformBroadcaster()
        #------------------------ 获取小乌龟1的位置 ------------------------#
        subscriber = rospy.Subscriber('/turtle1/pose', Pose, callback=callBack)
    
        rospy.spin()

demo01\_turtle2.py

#!/usr/bin/env python
    # coding:utf-8
    import rospy
    from turtlesim.srv import Spawn,SpawnRequest
    from tf.broadcaster import TransformBroadcaster
    from tf.listener import TransformListener
    from tf.transformations import quaternion_from_euler
    from turtlesim.msg import Pose
    from geometry_msgs.msg import Twist
    import math
    
    def callBack(pose):
        rospy.loginfo('receive turtle2 pose')
        # 坐标
        translation = (pose.x,pose.y,0)
        # 姿态 (0,0,pose.theta)
        rotation = quaternion_from_euler(0,0,pose.theta)
        # 发送给tf
        broadcaster.sendTransform(translation,rotation,rospy.Time().now(),'turtle2','world')
    
    if __name__ == '__main__':
        # 创建节点
        rospy.init_node("demo01_turtle2")
        # 创建小乌龟
        proxy = rospy.ServiceProxy('/spawn', Spawn)
        # 等待服务
        proxy.wait_for_service()
        # 数据
        request = SpawnRequest()
        request.x = 3.0
        request.y = 3.0
        request.theta = 0.0
        request.name = 'turtle2'
        # 调用服务
        res = proxy.call(request)
        # 创建tf发布者
        broadcaster = TransformBroadcaster()
        # 发布小乌龟2移动的publisher
        publisher = rospy.Publisher('/turtle2/cmd_vel', Twist)
        # 订阅小乌龟2的位置
        subscriber = rospy.Subscriber('/turtle2/pose', Pose, callback=callBack)
        # 创建listener
        listener = TransformListener()
        while not rospy.is_shutdown():
            try:
                # 坐标以及四元数
                translation,rotation = listener.lookupTransform("turtle2", "turtle1",rospy.Time(0))
                # 获取x和y
                x = translation[0]
                y = translation[1]
                # 计算小乌龟2和小乌龟1的距离
                distance = math.sqrt(pow(x,2)+pow(y,2))
                # 计算小乌龟1和2的夹角
                angle = math.atan2(y,x)
                # 角度范围在0-180 -180-0
                if(angle>math.pi):
                    angle -= 2*math.pi
                elif (angle<-math.pi):
                    angle += 2*math.pi
                # 数据
                twist = Twist()
                twist.linear.x = distance*0.6
                twist.angular.z = 6*angle
                # 发布数据
                publisher.publish(twist)
            except:
                pass
    
    
        rospy.spin()