Part4.3 ros中的坐标变换多坐标变换

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;
}