Part4.4ros中的坐标变换 实操

1.实现小乌龟追随案例
一只小乌龟在前面跑，另一只追在后面
2.launch文件集成

<launch>
<!--小乌龟生成和键盘控制节点-->
 <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" output="screen"/>
 <node pkg="operation" type="turtlesim_new" name="turtlesim_new" output="screen"/>
 <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen" />
 <!--发布两只乌龟的坐标系变化-->
 <node pkg="tf" type="pub_dynamic_tf" name="pub_dynamic_tf1" args="turtle1" output="screen"/>
 <node pkg="tf" type="pub_dynamic_tf" name="pub_dynamic_tf2" args="turtle2" output="screen"/>
 <!--定阅并解析两只乌龟的坐标关系-->
 <!-- turtle1相对于turtle2的位置关系，并发布速度消息-->
 <node pkg="tf" type="pub_more_frame" name="pub_more_frame"  output="screen"/>
 
 
</launch>

3.乌龟相对于world的坐标系发布

#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

*/
std::string turtle_name;
 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=turtle_name;
     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[])
{
   /*launch 文件通过args传入参数*/
   if(argc!=4) //通过launch传入的是4个字符串，argv【0】功能包路径 1 参数 2 程序名称 3 launch文件路径
   {
     ROS_ERROR("INFO ONE args");
     return 1;
   }
   else{
    turtle_name=argv[1];
    
   }
 ros::init(argc,argv,"dy_pub");
 ros::NodeHandle nh;
 ros::Subscriber sub=nh.subscribe(turtle_name + "/pose",100,dopose);//定阅乌龟相对于世界坐标系的关系
 ros::spin();     
return 0;
}

4.两个坐标系之间的位置关系以及速度的发布

#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" //组织发布坐标系之间的关系所需要的头文件
#include "geometry_msgs/Twist.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::Publisher pub=nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);
  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;
        geometry_msgs::TransformStamped turtlesim1toturtle2;
           //计算son1和son2的相对关系  
           //son1toson2=buffer.lookupTransform("son2","son1",ros::Time(0));// 该转换函数一共三个参数  参考坐标系 要转换的坐标系 时间设置为0是转换最近的两个坐标系
           turtlesim1toturtle2=buffer.lookupTransform("turtle2","turtle1",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;
         //ROS_INFO("6666666 %s %s", turtlesim1toturtle2.header.frame_id.c_str(), turtlesim1toturtle2.child_frame_id.c_str());
         //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);
        //组织速度发布消息
        //如何去计算速度，在z方向上的偏移量是反正切函数 在x上的速度是当x和y有距离时
        geometry_msgs::Twist twist;
        twist.angular.z=atan2(turtlesim1toturtle2.transform.translation.y,turtlesim1toturtle2.transform.translation.x);
        //twist.linear.x=sqrt(pow(turtlesim1toturtle2.transform.translation.x,2)+pow(turtlesim1toturtle2.transform.translation.y,2));
        twist.linear.x=turtlesim1toturtle2.transform.translation.x;
           pub.publish(twist);
    }
    catch(const std::exception& e)
    {
     ROS_INFO("erro :%s",e.what());
    }
      r.sleep();
      ros::spinOnce();
  }
    return 0;
}