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()