坐标转换
坐标转换是机器人学中的基本概念,因为机器人中存在大量的组件,每个组件有不同的坐标系。一个坐标系可以通过平移和旋转得到另一个坐标系,平移和旋转可以通过变换矩阵实现。有关这部分的知识可参阅:三维空间中刚体运动。
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 <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.编译并运行。
在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机器人开发实践.机械工业出版社