ROS-坐标转换
个人博客：http://www.chenjianqu.com/
原文链接：http://www.chenjianqu.com/show-73.html
坐标转换
坐标转换是机器人学中的基本概念，因为机器人中存在大量的组件，每个组件有不同的坐标系。一个坐标系可以通过平移和旋转得到另一个坐标系，平移和旋转可以通过变换矩阵实现。有关这部分的知识可参阅：三维空间中刚体运动。
ROS中坐标变换
下面是PR2机器人的坐标系一览：
其本质上可以抽象为一个坐标系树(tf tree):
ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系。l可以看到又很多的frame,错综复杂的铺置在机器人的各个link上，维护各个坐标系之间的关系，就要靠着tf tree来处理，维护着各个坐标系之间的联通。
每一个圆圈代表一个frame,对应着机器人上的一个link，任意的两个frame之间都必须是联通的。每两个frame之间都有一个broadcaster，为了使得两个frame之间能够正确连通，中间都会有一个Node来发布坐标转换信息。如果缺少Node来发布消息维护连通，那么这两个frame之间的连接就会断掉。broadcaster就是一个publisher，如果两个frame之间发生了相对运动，broadcaster就会发布相关消息。
坐标系中间的broadcaster发布是消息是TransformStampde.msg，内容如下：
std_mags/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w上面的消息中：header定义了序号，时间，frame的名称，child_frame的名称。这两个frame之间的变换由geometry_msgs/Transform来定义，Vector3三维向量表示平移，Quaternion四元数表示旋转。
一个机器人系统里有很多不同frame之间的broadcaster在发布坐标变换的tf，他们拼接得到tf tree。TF tree的数据类型是tf/tfMessage.msg（老版）和tf2_msgs/TFMessage.msg（新版）。标准格式如下：
首先header定义了序号，时间以及frame的名称．接着还写了child_frame，这两个frame之间要做那种变换就是由geometry_msgs/Transform来定义．Vector3三维向量表示平移，Quaternion四元数表示旋转
geometry_msgs/TransformStamped[] transforms
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w如上，tf tree就是TransformStamped数组。
ROS TF常见功能
TF功能包中提供了一些常用功能用来调试和创建TF变换。
1.tf_monitor
该节点可以查看坐标系广播的发布状态。
rosrun tf tf_monitor #查看所有广播器的发布
rosrun tf tf_monitor source_frame target_frame #查看两个坐标系的广播2.tf_echo
该节点可以查看指定坐标系之间的变换关系，如：
rosrun tf tf_echo turtle3 turtle1结果：
At time 1576577139.888
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.952, 0.306]
in RPY (radian) [-0.000, -0.000, 2.519]
in RPY (degree) [-0.000, -0.000, 144.307]3.static_transfrm_publisher
发布两个坐标系之间的静态坐标变换。
4.view_frames
生成pdf显示TF树。如：
rosrun tf view_frames结果：
Turtlesim功能包
turtlesim是ROS自带的一个用于新手教学的功能包，核心就是用键盘控制小乌龟运动。该功能包有两个节点，详细的信息可以参阅官方文档：http://wiki.ros.org/turtlesim 。
TF的turtlesim跟随实例
这里实现的是在turtlesim里面创建多个乌龟，每个乌龟的parent_frame都是世界坐标系，在TF tree上查找某两个乌龟之间的坐标变换，从而实现乌龟的跟随。
1.创建功能包
在工作空间创建功能包，依赖：roscpp tf turtlesim。这里创建的功能包为tf_test
2.节点：生成乌龟。
启动turtlesim turtlesim_node之后，只默认创建一个乌龟，名为/turtle1。这里通过调用turtlesim::Spawn服务，在仿真环境中可以创建新的乌龟。节点tf_test/src/turtle_create.cpp：
#include
#include
int main(int argc, char** argv)
{
ros::init(argc, argv, "node_create_turtle");
ros::NodeHandle node;
// 通过服务调用，产生第二只乌龟turtle2
ros::service::waitForService("spawn"); //等待spawn服务可用
ros::ServiceClient add_turtle =node.serviceClient("spawn");
turtlesim::Spawn srv;//默认参数
add_turtle.call(srv);
return 0;
};
3.节点：发布坐标转换
前面说过，每个frame之间必须要有一个广播节点发布坐标转换，这里的乌龟的坐标系在tf tree上面是世界坐标系的孩子。广播器如下(tf_test/src/turtle_tf_broadcaster.cpp)：
#include
#include
#include
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// tf广播器
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];
// 订阅乌龟的pose信息
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
}; 从官方文档可知，每个乌龟会发布消息turtle_name/pose，其内容turtlesim/Pose如下：
//乌龟在世界坐标系中的坐标
float32 x
float32 y
//乌龟在世界坐标系中，绕z轴旋转的角度
float32 theta
//线速度和角速度
float32 linear_velocity
float32 angular_velocity回调函数里面，首先定义一个广播器br，接着定义一个坐标变换transform。setOrigin函数设置平移变换，设坐标变换A->B，那么该函数就是设置B坐标系原点在A坐标系的坐标。接着定义一个四元数q。setRPY即把B坐标系的相对旋转角度设置为转换四元数。最后发布坐标变换。
4.节点：监听坐标变换，并将坐标转换用于控制乌龟运动。
通过当监听到广播器发出的坐标转换时，查找两个乌龟间的坐标变换关系，并将该坐标变换用于控制乌龟的运动，使一只乌龟能跟上另一只乌龟。因此这里还定义一个topic，叫做turtle_name/cmd_vel用于控制乌龟的运动。代码tf_test/src/turtle_tf_listener.cpp：
#include
#include
#include
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
if (argc != 3){
ROS_ERROR("need turtle name as argument");
return -1;
};
//A跟随B,获取乌龟A和乌龟B的名字
std::string turtleA = argv[1];
std::string turtleB = argv[2];
//定义turtle的速度控制发布器
ros::Publisher turtle_vel =node.advertise(turtleA+"/cmd_vel", 10);
//tf监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
// 查找turtleA与turtleB的坐标变换
listener.waitForTransform("/"+turtleA, "/"+turtleB, ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/"+turtleA, "/"+turtleB, ros::Time(0), transform);
}
catch (tf::TransformException &ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1和turtle2之间的坐标变换，计算turtle2需要运动的线速度和角速度
//并发布速度控制指令，使turtle2向turtle1移动
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;
}; 代码的解释可以看注释。
5.启动文件。
tf_test/launch/turtlesim_tf_test.launch
6.编译并运行。
在tf_test/CMakeLists.txt里面设置编译选项：
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
add_executable(turtle_create src/turtle_create.cpp)
target_link_libraries(turtle_create ${catkin_LIBRARIES})然后编译工作空间，编译完后直接运行launch文件即可查看效果。
结果如下：
可以看到键盘控制最左边乌龟的运动，中间的乌龟跟随左边的，右边的乌龟跟随中间的。
参考文献
[0]中科院软件所,重德智能公司.中国大学MOOC---《机器人操作系统入门》课程讲义.https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/
[1]胡春旭.ROS机器人开发实践.机械工业出版社