1.实现目标
相对与world坐标系,有两个子坐标系,已经知道子坐标系的位置,求son1相对于son2的位置
2.发布两个坐标系相对于world的位置,用launch文件实现
<launch>
<!--发布son1和son2相对于world的关系-->
<node pkg="tf2_ros" name="son1toworld" type="static_transform_publisher" args="5 0 0 0 0 0 /world /son1" output="screen" />
<node pkg="tf2_ros" name="son2toworld" type="static_transform_publisher" args="4 0 5 0 0 0 /world /son2" output="screen" />
</launch>
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"//坐标转换所需要的头文件
#include "geometry_msgs/TransformStamped.h" //组织发布坐标系之间的关系所需要的头文件
/*
计算son1和son2的相对关系
计算son1中的某个坐标点在son2的坐标值
*/
int main(int argc,char*argv[])
{
ros::init(argc,argv,"more_tf");
ros::NodeHandle nh;
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
ros::Rate r(10);
geometry_msgs::PointStamped psson1;
psson1.header.stamp= ros::Time::now();
psson1.header.frame_id="son1";
psson1.point.x=1;
psson1.point.y=2;
psson1.point.z=3;
while(ros::ok())
{
try
{
geometry_msgs::TransformStamped son1toson2;
//计算son1和son2的相对关系
son1toson2=buffer.lookupTransform("son2","son1",ros::Time(0));// 该转换函数一共三个参数 参考坐标系 要转换的坐标系 时间设置为0是转换最近的两个坐标系
ROS_INFO("son1 to son2 fatherframe: %s children : %s x: %.2f y : %.2f z :%.2f",son1toson2.header.frame_id.c_str(),son1toson2.child_frame_id.c_str(),son1toson2.transform.translation.x,son1toson2.transform.translation.y,son1toson2.transform.translation.z);
// 计算son1中的某个坐标点在son2的坐标值
geometry_msgs::PointStamped psson2;
psson2= buffer.transform(psson1,"son2");
ROS_INFO("point in son2 name= %s x= %.2f y= %.2f z= %.2f",psson2.header.frame_id.c_str(),psson2.point.x,psson2.point.y,psson2.point.z);
}
catch(const std::exception& e)
{
ROS_INFO("erro :%s",e.what());
}
r.sleep();
ros::spinOnce();
}
return 0;
}