ros之TF坐标转换

53 篇文章 0 订阅
43 篇文章 1 订阅

                                        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 跟着乌龟1走
            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()

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

_无往而不胜_

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值