6. ROS坐標系統

博客轉載：https://blog.csdn.net/hcx25909/article/details/9255001 和 https://www.cnblogs.com/wlzy/p/8214563.html

在機器人的控制中，坐標系統是非常重要的，在ROS使用tf軟件庫進行坐標轉換。

一、tf簡介

   我們通過一個小小的實例來介紹tf的作用。

1、安裝turtle包

sudo apt-get install ros-indigo-turtlebot

2、運行demo

$ roslaunch turtle_tf turtle_tf_demo.launch

然后就會看到兩只小烏龜

 

  該例程中帶有turtlesim仿真，可以在終端激活的情況下進行鍵盤控制。

 

方向鍵控制黃色烏龜的運動，第二只綠色烏龜會跟隨你移動的烏龜進行移動

3、demo分析

 這個例程使用tf建立了三個參考系：a world frame, a turtle1 frame, and a turtle2 frame。然后使用tf broadcaster發布烏龜的參考系，並且使用tf listener計算烏龜參考系之間的差異，使得第二只烏龜跟隨第一只烏龜。

我們可以使用tf工具來具體研究

rosrun tf view_frames

生成frames.pdf和frame.gv

digraph G {
"world" -> "turtle1"[label="Broadcaster: /turtle1_tf_broadcaster\nAverage rate: 62.700 Hz\nMost 
recent transform: 1587301448.302 ( 0.004 sec old)\nBuffer length: 4.976 sec\n"];
"world" -> "turtle2"[label="Broadcaster: /turtle2_tf_broadcaster\nAverage rate: 62.702 Hz\nMost 
recent transform: 1587301448.302 ( 0.003 sec old)\nBuffer length: 4.976 sec\n"];
edge [style=invis];
 subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
"Recorded at time: 1587301448.306"[ shape=plaintext ] ;
 }->"world";
}

該文件描述了參考系之間的聯系。三個節點分別是三個參考系，而/world是其他兩個烏龜參考系的父參考系。還包含一些調試需要的發送頻率、最近時間等信息。
tf還提供了一個tf_echo工具來查看兩個廣播參考系之間的關系。我們可以看一下第二只得烏龜坐標是怎么根據第一只烏龜得出來的。

rosrun tf tf_echo turtle1 turtle2

控制一只烏龜，在終端中會看到第二只烏龜的坐標轉換關系。

我們也可以通過rviz的圖形界面更加形象的看到這三者之間的關系。

 

 

移動烏龜，可以看到在rviz中的坐標會跟隨變化。其中左下角的是/world，其他兩個是烏龜的參考系,下面我們就來詳細分析這個實例。

二、Writing a tf broadcaster

1、創建包

cd /home/ke/Desktop/Planner/tf_ws/src/src/
catkin_create_pkg learning_tf tf roscpp rospy turtlesim

建立你的新包roscd之前:

cd ~/catkin_ws
catkin_make
source ./devel/setup.bash

我們首先創建源文件。 我們剛剛創建的包:　

roscd learning_tf

在src文件夾內創建turtle_tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
  
std::string turtle_name;
  
  
  
void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
  
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
  turtle_name = argv[1];
  
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  
  ros::spin();
  return 0;
};

在CMakeLists.txt內添加

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

之后 catkin_make編譯， 創建launch文件start_demo.launch：

<launch>
  <!-- Turtlesim Node-->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
 
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
  <!-- Axes -->
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>
 
  <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
  <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
 
</launch>

運行

roslaunch learning_tf start_demo.launch

 可以看到界面中只有移植烏龜了，打開tf_echo的信息窗口：

rosrun tf tf_echo /world /turtle1 

world參考系的原點在最下角，對於turtle1的轉換關系，其實就是turtle1在world參考系中所在的坐標位置以及旋轉角度。

三、Writing a tf listener

這一步，我們將看到如何使用tf進行參考系轉換。首先寫一個tf listener: turtle_tf_listener.cpp。The turtlesim/Velocity.h header is not used anymore(再也不), it has been replaced by geometry_msgs/Twist.h. Furthermore(此外), the topic/turtle/command_velocity is now called /turtle/cmd_vel. In light of this, a few changes are necessary to make it work

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
 
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");
 
  ros::NodeHandle node;
 
  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);
 
  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
 
  tf::TransformListener listener;
 
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
 
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                    transform.getOrigin().x());
    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                  pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);
 
    rate.sleep();
  }
  return 0;
};

修改 CMakeLists.txt

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

再次編譯即可: catkin_make

修改launch文件start_demo.launch添加：

<launch>
  ...
  <node pkg="learning_tf" type="turtle_tf_listener"
        name="listener" />
</launch>

然后在運行：

roslaunch learning_tf start_demo.launch 

就可以看到兩只turtle了，也就是我們在最開始見到的那種跟隨效果。

四、Adding a frame

在很多應用中，添加一個參考系是很有必要的，比如在一個world參考系下，有很一個激光掃描節點，tf可以幫助我們將激光掃描的信息坐標裝換成全局坐標。

1、tf消息結構

tf中的信息是一個樹狀的結構，world參考系是最頂端的父參考系，其他的參考系都需要向下延伸。如果我們在上文的基礎上添加一個參考系，就需要讓這個新的參考系成為已有三個參考系中的一個的子參考系。

2、建立固定參考系（fixed frame）

 我們以turtle1作為父參考系，建立一個新的參考系carrot1, 添加文件frame_tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
  
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;
  
  tf::TransformBroadcaster br;
  tf::Transform transform;
  
  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

修改 CMakeLists.txt

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

修改launch文件start_demo.launch添加：

<launch>
  ...
  <node pkg="learning_tf" type="frame_tf_broadcaster"
        name="broadcaster_frame" />
</launch>

重新編譯 catkin_make,   然后運行：

 roslaunch learning_tf start_demo.launch 

 運行，還是看到兩只烏龜和之前的效果一樣. 新添加的參考系並沒有對其他參考系產生什么影響.Open the src/turtle_tf_listener.cpp file, and simple replace "/turtle1" with "/carrot1" in lines 26-27:

listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);

重新運行，現在烏龜之間的跟隨關系就改變了：

3、建立移動參考系（moving frame）

我們建立的新參考系是一個固定的參考系，在仿真過程中不會改變，如果我們要把carrot1參考系和turtle1參考系之間的關系設置可變的，可以修改代碼如下：

#include <cmath>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
  
int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;
  
  tf::TransformBroadcaster br;
  tf::Transform transform;
  
  ros::Rate rate(10.0);
  while (node.ok())
  {
  
    double secs = ros::Time::now().toSec();
    transform.setOrigin( tf::Vector3(2.0 * cos(secs), 2.0 * sin(secs), 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

 這次carrot1的位置現對於turtle1來說是一個三角函數關系了。