Part4.2 ros中的坐标变换 动态坐标变换

1.实现目标 实时接受小乌龟坐标系相对于word坐标系的位置关系
然后把乌龟坐标系中的点转变为world坐标系中的点
2.发布方实现

#include "ros/ros.h"
#include "turtlesim/Pose.h" //乌龟姿态的消息类型
#include "tf2_ros/transform_broadcaster.h" //发布对象依赖的头文件
#include "geometry_msgs/TransformStamped.h" //组织发布坐标系之间的关系所需要的头文件
#include "tf2/LinearMath/Quaternion.h" //设置四元数所需要的头文件,用来把欧拉角转变成四元数
/*
 定阅乌龟的位置姿态信息，然后转换成相对于窗体的坐标关系，并发布
 准备工作：
   话题:/turtle1/pose

   消息：turtlesim/Pose
   float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

*/
 void dopose(const turtlesim::Pose::ConstPtr& pose )
 {
     //获取位姿态信息，转换成坐标系的相对关系，并发布
     static tf2_ros::TransformBroadcaster pub;//创建发布对象
     geometry_msgs::TransformStamped ts;//组织被发布的数据
     ts.header.frame_id="world";
     ts.header.stamp=ros::Time::now();
     ts.child_frame_id="turtlesim1";
     ts.transform.translation.x=pose->x;
     ts.transform.translation.y=pose->y;
     ts.transform.translation.z=0;
      //设置欧拉角转换为四元数
  tf2::Quaternion qtn;
  //向该对象设置欧拉角
  qtn.setRPY(0,0,pose->theta); //其中的参数是在xyz轴上的旋转量，单位是弧度
  //设置四元数
  ts.transform.rotation.x=qtn.getX();
  ts.transform.rotation.y=qtn.getY();
  ts.transform.rotation.z=qtn.getZ();
  ts.transform.rotation.w=qtn.getW();
   //发布数据
  pub.sendTransform(ts);
 }
  
int main(int argc,char *argv[])
{
 ros::init(argc,argv,"dy_pub");
 ros::NodeHandle nh;
 ros::Subscriber sub=nh.subscribe("/turtle1/pose",100,dopose);//定阅乌龟相对于世界坐标系的关系
 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_dynamic");
  ros::NodeHandle nh;
  tf2_ros::Buffer buffer;
  tf2_ros::TransformListener listener(buffer);//创建定阅对象
  geometry_msgs::PointStamped ps; //创建坐标点
  ps.header.frame_id="turtlesim1";
  ps.header.stamp=ros::Time(0.0);//ros会拿着此时的时间去比对定阅到的时间辍，如果使用ros：：TIME：：now 会使得ros认为buffer中的消息是很早前传过来的，没有意义不进行转换。
  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,"world");//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;
}