ROS理解：ros中的坐标以及对tf2进行解读

官网就是最好的教程，如果阅读英文没什么压力，强烈推荐以下链接进行全面了解：

官网tf2教程

1. ROS中的坐标

机器人当中有一套自己的坐标标准，它的直角坐标系表示如下：

相对于机器人来说：

X ：朝前 ，红色
Y ：朝左，绿色
Z ：朝上，蓝色

旋转变换的坐标表示如下：

其中：Roll，Pitch，Yaw的右手法则，大拇指的方向为X，Y，Z的方向。

怎么理解发布的坐标呢？

建立父link到子link的tf变换，就是让父link向子link移动过程中（x, y, z, Roll，Pitch，Yaw）的变化。

2. tf到tf2的变化

tf已经被ROS弃用了，而且tf2包含了tf的所有功能，所以这里不介绍tf了，如果想要了解tf到tf2的变化，可以点击下列链接。

数据类型的变化：http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions
监听模式的变化：http://wiki.ros.org/tf2/Tutorials/Migration/TransformListener
发布模式的变化：http://wiki.ros.org/tf2/Tutorials/Migration/TransformBroadcaster

3. 发布静态tf2

静态tf是不随时间变化的坐标变换，可以有三种发布方式，：launch方式、urdf发布方式和写代码方式，下面分别介绍。

• 方式一：launch（或run）文件方式发布

这种方式最简单直接，直接发布从父link到子link的坐标变换，代码如下：

1） 欧拉角发布

1. launch方法

<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z yaw pitch roll link1 link2 100" />
</launch>

2. run方法

# 欧拉角
rosrun tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

2） 四元数发布

1. launch方法

<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z qx qy qz qw link1 link2 100" />
</launch>

2. run方法

# 四元数
rosrun tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

• x y z 代表link2相对与link1的位置变换。

• qx qy qz qw表示link2相对与link1的位姿变换的四元数。

• 参数100表示每隔100毫秒发布一次。

• 方式二：urdf方式发布
  待添加
• 方式三：代码方式发布

创建ros包：

catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

代码以及注释如下：

#include 
#include 
#include 
#include 
#include 
#include 
std::string static_turtle_name;

int main(int argc, char **argv)
{
  ros::init(argc,argv, "my_static_tf2_broadcaster");
  if(argc != 8)//需要输入8个参数
  {
    ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
    return -1;
  }
  if(strcmp(argv[1],"world")==0)//1号参数不能和world相同，因为它是子link
  {
    ROS_ERROR("Your static turtle name cannot be 'world'");
    return -1;

  }
  static_turtle_name = argv[1];//赋值给子link，argv[0]为命令节点
  static tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped static_transformStamped;

  static_transformStamped.header.stamp = ros::Time::now();
  static_transformStamped.header.frame_id = "world";
  static_transformStamped.child_frame_id = static_turtle_name;

  //将char*类型的值转换为double类型，从第2号开始输入值
  //前三位为位置变换
  static_transformStamped.transform.translation.x = atof(argv[2]);
  static_transformStamped.transform.translation.y = atof(argv[3]);
  static_transformStamped.transform.translation.z = atof(argv[4]);
  tf2::Quaternion quat;
  //后三位为旋转变换
  quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
  static_transformStamped.transform.rotation.x = quat.x();
  static_transformStamped.transform.rotation.y = quat.y();
  static_transformStamped.transform.rotation.z = quat.z();
  static_transformStamped.transform.rotation.w = quat.w();
  static_broadcaster.sendTransform(static_transformStamped);
  ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());
  ros::spin();
  return 0;
};

在CMakeLists.txt下添加：

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
target_link_libraries(static_turtle_tf2_broadcaster  ${catkin_LIBRARIES} )

运行：

rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

以上两种方式都可以通过命令查看发布的静态tf2变换，静态tf2的话题为/tf_static，查看静态tf命令如下：

rostopic echo /tf_static

或者使用rqt查看tf树：

---

4. 发布动态tf2

因为ros的小乌龟可以使用键盘控制它的移动，所以我们尝试订阅一个小乌龟位姿，并发布它到world的tf。

直接上代码和注释：

#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;

std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf2_ros::TransformBroadcaster br;
  geometry_msgs::TransformStamped transformStamped;
  
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  transformStamped.child_frame_id = turtle_name;
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  br.sendTransform(transformStamped);//发布tf
}
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");
  ros::NodeHandle private_node("~");
  if (! private_node.hasParam("turtle"))
  {
    if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
    turtle_name = argv[1];
    cout<<turtle_name<<endl;
  }
  else
  {
    private_node.getParam("turtle", turtle_name);
  }
  ros::NodeHandle node;
  //订阅乌龟的位姿，系统发布出来的是小乌龟话题为：/turtle1/pose，键盘：/turtle1/cmd_vel
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  ros::spin();
  return 0;
};

注意： 模拟的小乌龟位姿为：/turtle1/pose，模拟键盘为：/turtle1/cmd_vel。所以这里只订阅了一个小乌龟。

添加CMakeList.txt文件

add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
target_link_libraries(turtle_tf2_broadcaster
 ${catkin_LIBRARIES}
)

在包的src同一目录添加launch文件，并在它的文件内添加一下内容：

  <launch>
     <!-- 模拟乌龟接口-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
	<!-- 模拟键盘控制-->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- 键盘控制的线速度与角速度变化尺度 -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>
    
	<!-- 启动两个节点，它们的可执行文件同名-->
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />
  </launch>

注意：

1. 虽然是两个节点，但是生成的可执行文件相同。
2. args为传入的不同话题参数，但是只有turtle1/pose会被订阅。

加入launch文件配置，这样就可以直接运行相应包下的launch文件了。

例如：roslaunch learning_tf2 start_demo.launch

install(FILES
 start_demo.launch
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

查看tf变换：

rosrun tf tf_echo /world /turtle1

这里同样可以采用rqt下的tf树进行查阅。

---

5. 监听tf2

上面我们开启了两个节点进行订阅，其中一个订阅了小乌龟，另一个也有订阅接口但是没有发布者。所以我们尝试添加一只小乌龟和一个控制指令，定义他们的话题名为turtle2/pose和turtle/cmd_vel。这样的话就建立了从turtle2到world的tf。我们尝试让turtle2随着turtle1移动。

因为turtle1和turtle2都建立了到world的tf，所以我们可以监听turtle1到turtle2的tf来实现我们的需求。

直接上代码和注释：

#include 
#include 
#include 
#include 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_listener");
  ros::NodeHandle node;
  ros::service::waitForService("spawn");
  ros::ServiceClient spawner =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn turtle;
  turtle.request.x = 4;
  turtle.request.y = 2;
  turtle.request.theta = 0;
  turtle.request.name = "turtle2";
  spawner.call(turtle);//使用服务器来开启另一只小乌龟。

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);//小乌龟的控制指令

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  ros::Rate rate(10.0);
  while (node.ok()){
    geometry_msgs::TransformStamped transformStamped;
    try{
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                               ros::Time(0));//监听turtle2到turtle1到变换，不能监听ros::Time::now()
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    geometry_msgs::Twist vel_msg;
    //根据两只小乌龟的相对位姿来定义控制命令
    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                  pow(transformStamped.transform.translation.y, 2));
    turtle_vel.publish(vel_msg);
    rate.sleep();
  }
  return 0;
};

上述代码中请注意：

1. 我们只能订阅最新的tf变换，而不能订阅当下的tf变换，也就是说只能监听ros::Time(0)时刻最新的，而不能监听ros::Time::now()，因为建立tf是需要时间的。
2. 我们还可以设置等待项目(等待这一时刻tf建立)，它是一个可选项，如果设置了等待项目，则可以用now();如：
   transformStamped = tfBuffer.lookupTransform(“turtle2”, “turtle1”, ros::Time::now(),
   ros::Duration(3.0));

在上一个start_launch.launch中添加：

  <launch>
    ...
    <node pkg="learning_tf2" type="turtle_tf2_listener"
          name="listener" />
  </launch>

运行：

roslaunch learning_tf2 start_demo.launch

刚启动launch的时候会出现以下界面，这是因为刚开始没有turtle2的变换，需要等到服务建立起来后，才能监听trutle2的变换。

---

6. 增加自己的frame

其实上文懂了的话，就没必要看这一节了，因为上面已经创建过turtle1和turtle2的frame了。

代码与注释：

#include 
#include 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");
  ros::NodeHandle node;

   tf2_ros::TransformBroadcaster tfb;
  geometry_msgs::TransformStamped transformStamped;
  
  transformStamped.header.frame_id = "turtle1";//建立自己的frame与已知frame之间的tf
  transformStamped.child_frame_id = "carrot1";
  transformStamped.transform.translation.x = 0.0;
  transformStamped.transform.translation.y = 2.0;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
        q.setRPY(0, 0, 0);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  ros::Rate rate(10.0);
  while (node.ok()){
    transformStamped.header.stamp = ros::Time::now();
    tfb.sendTransform(transformStamped);//发布tf
    rate.sleep();
    printf("sending\n");
  }
};

添加CMakeList.txt文件：

add_executable(frame_tf2_broadcaster src/frame_tf2_broadcaster.cpp)
target_link_libraries(frame_tf2_broadcaster
 ${catkin_LIBRARIES}
)

运行：

roslaunch learning_tf2 start_demo.launch

使用rqt查看建立起来的tf树：

上面建立的是静态tf，我们还可以修改代码建立动态tf：

transformStamped.transform.translation.x = 2.0*sin(ros::Time::now().toSec());
transformStamped.transform.translation.y = 2.0*cos(ros::Time::now().toSec());

---