Part4.1ros中的坐标变换 静态坐标变换

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