Personal blog: http://www.chenjianqu.com/
Original link: http://www.chenjianqu.com/show-73.html
Coordinate transformation
Coordinate conversion is a fundamental concept in robotics, robots because of the presence of large number of components, each component has a different coordinate systems. A coordinate system can be obtained by another coordinate system translation and rotation, translation and rotation may be achieved by the transformation matrix. Knowledge about this part can be found: three-dimensional space rigid body motion .
ROS Coordinate Transformation
The following is a list of PR2 robot coordinate system:
Which is essentially a coordinate system can be abstracted tree (tf tree):
ROS robot model contains a lot of components, which is called link system, each link corresponding to a Frame above, i.e. a coordinate system. l and many see Frame, complex laid on each link of the robot, maintains the relationships between various coordinate systems, it is necessary to rely tf tree processing, maintains communication between the respective coordinate systems.
Each circle represents a frame , on a corresponding robot Link , between any two frame must be Unicom. Between each of the two has a Frame details for broadcast , in order to enable communication between the two right frame, an intermediate will have to release a Node coordinate conversion information. In the absence of Node to post messages to maintain connectivity, the connection between the two frame will be cut off. broadcaster is a publisher, if the relative motion between the two frame, broadcaster will release relevant information.
Intermediate coordinate system is a release message broadcaster is TransformStampde.msg, as follows:
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 <ros/ros.h>
#include <turtlesim/Spawn.h>
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<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;//默认参数
add_turtle.call(srv);
return 0;
};
3.节点：发布坐标转换
前面说过，每个frame之间必须要有一个广播节点发布坐标转换，这里的乌龟的坐标系在tf tree上面是世界坐标系的孩子。广播器如下(tf_test/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)
{
// 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 <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
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<geometry_msgs::Twist>(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
<launch>
<!-- 乌龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!--再另外创建两只乌龟-->
<node pkg="tf_test" type="turtle_create" name="turtle_create2" />
<node pkg="tf_test" type="turtle_create" name="turtle_create3" />
<!-- 三只乌龟的tf广播 -->
<node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
<node pkg="tf_test" type="turtle_tf_broadcaster" args="/turtle3" name="turtle3_tf_broadcaster" />
<!-- 监听tf广播，并且控制turtle移动 -->
<node pkg="tf_test" type="turtle_tf_listener" args="turtle2 turtle1" name="listener2" />
<node pkg="tf_test" type="turtle_tf_listener" args="turtle3 turtle2" name="listener3" />
</launch>
6.编译并运行。
Set compiler option in tf_test / CMakeLists.txt inside:
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})Then compile workspace after the compiler run directly launch the file to see the results.
The results are as follows:
You can see the keyboard to control the movement of the left tortoise, turtles follow the middle of the left side, the right to follow the turtle in the middle.
references
[0] CAS software, heavy German intelligence company. China University of MOOC --- "Android OS Getting Started" course notes. Https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/
[1] Huchun Xu .ROS robot development practices. Machinery Industry Press
