第 5 章 ROS常用组件动态坐标变换(自学二刷笔记)-编程知识

5.1.3 动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

需求描述:

启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

结果演示:

实现分析:

乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
订阅 turtle1/pose，可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
将 pose 信息转换成坐标系相对信息并发布

实现流程：C++ 与 Python 实现流程一致

新建功能包，添加依赖
创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
创建坐标相对关系订阅方
执行

方案A：C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

/*  动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)需求: 启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布实现分析:1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度3.将 pose 信息转换成 坐标系相对信息并发布实现流程:1.包含头文件2.初始化 ROS 节点3.创建 ROS 句柄4.创建订阅对象5.回调函数处理订阅到的数据(实现TF广播)5-1.创建 TF 广播器5-2.创建 广播的数据(通过 pose 设置)5-3.广播器发布数据6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"void doPose(const turtlesim::Pose::ConstPtr& pose){//  5-1.创建 TF 广播器static tf2_ros::TransformBroadcaster broadcaster;//  5-2.创建 广播的数据(通过 pose 设置)geometry_msgs::TransformStamped tfs;//  |----头设置tfs.header.frame_id = "world";tfs.header.stamp = ros::Time::now();//  |----坐标系 IDtfs.child_frame_id = "turtle1";//  |----坐标系相对信息设置tfs.transform.translation.x = pose->x;tfs.transform.translation.y = pose->y;tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0//  |--------- 四元数设置tf2::Quaternion qtn;qtn.setRPY(0,0,pose->theta);tfs.transform.rotation.x = qtn.getX();tfs.transform.rotation.y = qtn.getY();tfs.transform.rotation.z = qtn.getZ();tfs.transform.rotation.w = qtn.getW();//  5-3.广播器发布数据broadcaster.sendTransform(tfs);
}int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ROS 节点ros::init(argc,argv,"dynamic_tf_pub");// 3.创建 ROS 句柄ros::NodeHandle nh;// 4.创建订阅对象ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);//     5.回调函数处理订阅到的数据(实现TF广播)//        // 6.spinros::spin();return 0;
}

配置文件此处略。

3.订阅方

//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ROS 节点ros::init(argc,argv,"dynamic_tf_sub");ros::NodeHandle nh;// 3.创建 TF 订阅节点tf2_ros::Buffer buffer;tf2_ros::TransformListener listener(buffer);ros::Rate r(1);while (ros::ok()){// 4.生成一个坐标点(相对于子级坐标系)geometry_msgs::PointStamped point_laser;point_laser.header.frame_id = "turtle1";point_laser.header.stamp = ros::Time();point_laser.point.x = 1;point_laser.point.y = 1;point_laser.point.z = 0;// 5.转换坐标点(相对于父级坐标系)//新建一个坐标点，用于接收转换结果  //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------try{geometry_msgs::PointStamped point_base;point_base = buffer.transform(point_laser,"world");ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);}catch(const std::exception& e){// std::cerr << e.what() << '\n';ROS_INFO("程序异常:%s",e.what());}r.sleep();  ros::spinOnce();}return 0;
}

配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。

可以使用 rviz 查看坐标系相对关系。

方案B：Python实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

#! /usr/bin/env python
"""  动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布实现分析:1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度3.将 pose 信息转换成 坐标系相对信息并发布实现流程:1.导包2.初始化 ROS 节点3.订阅 /turtle1/pose 话题消息4.回调函数处理4-1.创建 TF 广播器4-2.创建 广播的数据(通过 pose 设置)4-3.广播器发布数据5.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped#     4.回调函数处理
def doPose(pose):#         4-1.创建 TF 广播器broadcaster = tf2_ros.TransformBroadcaster()#         4-2.创建 广播的数据(通过 pose 设置)tfs = TransformStamped()tfs.header.frame_id = "world"tfs.header.stamp = rospy.Time.now()tfs.child_frame_id = "turtle1"tfs.transform.translation.x = pose.xtfs.transform.translation.y = pose.ytfs.transform.translation.z = 0.0qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)tfs.transform.rotation.x = qtn[0]tfs.transform.rotation.y = qtn[1]tfs.transform.rotation.z = qtn[2]tfs.transform.rotation.w = qtn[3]#         4-3.广播器发布数据broadcaster.sendTransform(tfs)if __name__ == "__main__":# 2.初始化 ROS 节点rospy.init_node("dynamic_tf_pub_p")# 3.订阅 /turtle1/pose 话题消息sub = rospy.Subscriber("/turtle1/pose",Pose,doPose)#     4.回调函数处理#         4-1.创建 TF 广播器#         4-2.创建 广播的数据(通过 pose 设置)#         4-3.广播器发布数据#     5.spinrospy.spin()

权限设置以及配置文件此处略。

3.订阅方

#! /usr/bin/env python
"""  动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布实现分析:1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度3.将 pose 信息转换成 坐标系相对信息并发布实现流程:1.导包2.初始化 ROS 节点3.创建 TF 订阅对象4.处理订阅的数据"""
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStampedif __name__ == "__main__":# 2.初始化 ROS 节点rospy.init_node("static_sub_tf_p")# 3.创建 TF 订阅对象buffer = tf2_ros.Buffer()listener = tf2_ros.TransformListener(buffer)rate = rospy.Rate(1)while not rospy.is_shutdown():    # 4.创建一个 radar 坐标系中的坐标点point_source = PointStamped()point_source.header.frame_id = "turtle1"point_source.header.stamp = rospy.Time.now()point_source.point.x = 10point_source.point.y = 2point_source.point.z = 3try:#     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标point_target = buffer.transform(point_source,"world",rospy.Duration(1))rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",point_target.point.x,point_target.point.y,point_target.point.z)except Exception as e:rospy.logerr("异常:%s",e)#     6.spinrate.sleep()

权限设置以及配置文件此处略。