1.简述
本例来实现一个座标点在另一个坐标系中的转换,其中转换函数ros已经给封装好了。
依赖的功能包:tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.发布方的实现
去发布两个坐标系之间的关系
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h" //发布对象依赖的头文件
#include "geometry_msgs/TransformStamped.h" //组织发布坐标系之间的关系所需要的头文件
#include "tf2/LinearMath/Quaternion.h" //设置四元数所需要的头文件,用来把欧拉角转变成四元数
/*
发布两个坐标系的相对关系
包含头文件
初始化节点
创建发布对象
组织被发布的消息
发布数据
spin()
*/
int main(int argc,char *argv[])
{
ros::init(argc,argv,"st_tf");
ros::NodeHandle nh;
tf2_ros::StaticTransformBroadcaster pub;//创建发布对象
geometry_msgs::TransformStamped tfs; //组织被发布的数据
tfs.header.stamp=ros::Time::now(); //设置时间辍,设置为ros系统的当前时间
tfs.header.frame_id="base_link" ;//设置被参考的坐标系名称(父系坐标系)
tfs.child_frame_id="laser_link"; //设置子系坐标系
tfs.transform.translation.x=0.2;//设置子坐标系在父坐标系的xyz方向上的偏移量
tfs.transform.translation.y=0.1;
tfs.transform.translation.z=0.2;
//设置欧拉角转换为四元数
tf2::Quaternion qtn;
//向该对象设置欧拉角
qtn.setRPY(0,0,0); //其中的参数是在xyz轴上的旋转量,单位是弧度
//设置四元数
tfs.transform.rotation.x=qtn.getX();
tfs.transform.rotation.y=qtn.getY();
tfs.transform.rotation.z=qtn.getZ();
tfs.transform.rotation.w=qtn.getW();
//发布数据
pub.sendTransform(tfs);
ros::spin(); //回调函数,循环发布坐标信息
return 0;
}
3.定阅方实现
对发布的坐标系关系进行定阅,并进行坐标转换
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h" //创建定阅对象所需的头文件
#include "tf2_ros/buffer.h"//可以把定阅的数据放到buffer中
#include "geometry_msgs/PointStamped.h"//用于创建坐标点对象
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"//坐标转换所需要的头文件
/*
定阅方实现
定阅发布方发出的坐标系相对关系,并传入一个坐标点(子坐标系),计算其在父坐标系的坐标
流程:
头文件
节点初始化,编码初始化,nodehandle
创建一个定阅对象,定阅坐标系相对关系
组织坐标点数据
转换算法实现,调用tf内置实现
最后输出转完的坐标系
*/
int main(int argc,char *argv[])
{
ros::init(argc,argv,"sub_static");
ros::NodeHandle nh;
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);//创建定阅对象
geometry_msgs::PointStamped ps; //创建坐标点
ps.header.frame_id="laser_link";
ps.header.stamp=ros::Time::now();
ps.point.x=2.0;
ps.point.y=3.0;
ps.point.z=4.0;
//ros::Duration(2).sleep();// 为了确保定阅的数据已经存到buffer中了,给一个时间缓冲
ros::Rate r(10);//转换频率
while(ros::ok())
{
try
{
/* code */
geometry_msgs::PointStamped psout;
psout=buffer.transform(ps,"base_link");//buffer里面有两个参数,一个是需要转换的坐标点,另一个是要转换到哪个坐标系;
ROS_INFO("the point fram=%s x= %.2f,y= %.2f, z= %.2f",psout.header.frame_id.c_str(), psout.point.x,psout.point.y,psout.point.z);
r.sleep();
ros::spinOnce();
}
catch(const std::exception& e)
{
ROS_INFO("%s",e.what());
}
//转换代码的实现,ros内部已经封装好了
}
return 0;
}