当前位置：首页 > news >正文

ros0基础-day17

news 2025/7/23 14:33:52

1、动态坐标变换

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

需求描述:

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

实现分析:

乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点

订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度

将 pose 信息转换成坐标系相对信息并发布

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

新建功能包，添加依赖

创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)

创建坐标相对关系订阅方

执行

1.1、C++实现

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

配置

发布方

#include "ros/ros.h"  // 引入 ROS 的基础库
#include "turtlesim/Pose.h"  // 引入乌龟位姿消息类型
#include "tf2_ros/transform_broadcaster.h"  // 引入 tf2 广播器库，用于发布坐标变换
#include "geometry_msgs/TransformStamped.h"  // 引入转换消息类型，用于发布位姿信息
#include "tf2/LinearMath/Quaternion.h"  // 引入四元数库，用于处理旋转/*发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布准 备：话题：/turtle1/pose消息：/turtlesim/Pose流程：1.包含头文件2.设置编码、初始化、NodeHandle3.创建订阅对象，订阅 /turtle1/pose4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布（关注）5.spin()
*/void doPose(const turtlesim::Pose::ConstPtr& pose)
{// 获取位姿信息，转换成坐标相对关系（核心），并发布// a. 创建发布对象static tf2_ros::TransformBroadcaster pub;  // 创建一个广播器，用于发布坐标变换信息// b. 组织被发布的数据geometry_msgs::TransformStamped ts;  // 创建一个TransformStamped消息，用于存储转换信息ts.header.frame_id = "world";  // 设置参考坐标系为“world”ts.header.stamp = ros::Time::now();  // 设置当前时间戳ts.child_frame_id = "turtle1";  // 设置子坐标系为“turtle1”// 坐标系偏移量设置ts.transform.translation.x = pose->x;  // 设置乌龟的 x 坐标ts.transform.translation.y = pose->y;  // 设置乌龟的 y 坐标ts.transform.translation.z = 0;  // 因为是二维问题，z 坐标设置为 0// 坐标系四元数/*位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是 2D，没有翻滚与俯仰角度，所以可以得出乌龟的欧拉角：0，0，theta*/tf2::Quaternion qtn;  // 创建四元数对象qtn.setRPY(0, 0, pose->theta);  // 设定四元数的欧拉角，只有偏航角（theta）有值，其他为 0ts.transform.rotation.x = qtn.getX();  // 设置四元数的 x 分量ts.transform.rotation.y = qtn.getY();  // 设置四元数的 y 分量ts.transform.rotation.z = qtn.getZ();  // 设置四元数的 z 分量ts.transform.rotation.w = qtn.getW();  // 设置四元数的 w 分量// c. 发布转换信息pub.sendTransform(ts);  // 发布转换信息
}int main(int argc, char *argv[])
{// 2. 设置编码、初始化、NodeHandlesetlocale(LC_ALL,"");  // 设置本地化（可以处理中文输出等）ros::init(argc, argv, "dynamic_pub");  // 初始化 ROS 节点，节点名为 dynamic_pubros::NodeHandle nh;  // 创建节点句柄，用于与 ROS 系统交互// 3. 创建订阅对象，订阅 /turtle1/poseros::Subscriber sub = nh.subscribe("/turtle1/pose", 100, doPose);  // 订阅“/turtle1/pose”话题，最多缓存 100 条消息，回调函数为 doPose// 4. 回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布（关注）// 5. spin()ros::spin();  // 启动 ROS 事件处理循环，回调函数会在这里被执行return 0;  // 返回 0，表示程序正常结束
}

发布方动态位姿信息变化

订阅方

#include "ros/ros.h"  // 引入 ROS 的基础库
#include "tf2_ros/transform_listener.h"  // 引入 TF2 监听器库，用于监听坐标变换
#include "tf2_ros/buffer.h"  // 引入 TF2 缓存库，用于存储坐标变换信息
#include "geometry_msgs/PointStamped.h"  // 引入点数据消息类型，包含坐标信息
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  // 引入 TF2 转换函数，用于坐标系之间转换/*    订阅方：订阅发布的坐标相对关系，传入tf实现转换流程：1.包含头文件2.编码 初始化 NodeHandle（必须的）3.创建订阅对象； ----> 订阅坐标系相对关系4.组织一个坐标点数据；5.转换算法，需要调用TF内置实现6.最后输出
*/int main(int argc, char *argv[])
{// 2.编码 初始化 NodeHandle（必须的）setlocale(LC_ALL,"");  // 设置本地化，可以正确显示中文等字符ros::init(argc,argv,"dynamic_sub");  // 初始化 ROS 节点，节点名为 dynamic_subros::NodeHandle nh;  // 创建节点句柄，用于与 ROS 系统交互// 3.创建订阅对象； ----> 订阅坐标系相对关系// 3-1. 创建一个 buffer 缓存tf2_ros::Buffer buffer;  // 创建 tf2 缓存对象，用于存储从其他节点接收到的坐标变换信息// 3-2. 再创建监听对象（监听对象可以将订阅的数据存入 buffer）tf2_ros::TransformListener listener(buffer);  // 创建 tf2 监听器，监听坐标变换，并将其存入 buffer// 4. 组织一个坐标点数据；geometry_msgs::PointStamped ps;  // 创建一个点数据消息对象// 参考的坐标系ps.header.frame_id = "turtle1";  // 设置参考坐标系为“turtle1”// 时间戳ps.header.stamp = ros::Time(0.0);  // 设置时间戳为 0（表示当前时间）ps.point.x = 2.0;  // 设置点的 x 坐标ps.point.y = 3.0;  // 设置点的 y 坐标ps.point.z = 5.0;  // 设置点的 z 坐标// 添加休眠// ros::Duration(5).sleep();  // （注释掉的代码）可以选择在此处让节点休眠 5 秒，等待 tf2 数据的准备// 5. 转换算法，需要调用 TF 内置实现ros::Rate rate(1);  // 设置循环频率为 1Hzwhile (ros::ok()){// 核心代码   ---- 将 ps 转换成相对于 base_link 的坐标点geometry_msgs::PointStamped ps_out;  // 创建一个输出坐标点消息对象/*调用了 buffer 转换函数 transform参数1：被转换的坐标点参数2：目标坐标系返回值：输出的坐标点ps1：调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.hps2：运行时候存在的问题：抛出一个异常 base_link不存在原因：订阅数据是一个耗时操作，可能在调用 transform 转换函数时，坐标系的相对关系还没有订阅到，因此出现异常解决：方案1：在调用转换函数前，执行休眠方案2：进行异常处理*/try{ps_out = buffer.transform(ps, "world");  // 将 ps 坐标点从 "turtle1" 坐标系转换到 "world" 坐标系// 6. 最后输出ROS_INFO("转换后的坐标值为：（%.2f, %.2f, %.2f）,参考的坐标系：%s",ps_out.point.x, ps_out.point.y, ps_out.point.z, ps_out.header.frame_id.c_str());  // 输出转换后的坐标信息}catch(const std::exception& e)  // 捕获异常，如果转换失败，输出异常信息{// std::cerr << e.what() << '\n';  // 输出异常信息到标准错误流ROS_INFO("异常消息：%s", e.what());  // 输出异常信息到 ROS log}  rate.sleep();  // 按照设定的频率（1Hz）休眠ros::spinOnce();  // 处理一次回调函数（如果有）}return 0;  // 返回 0，表示程序正常结束
}

发布方订阅方动态坐标变换

1.2、Python实现

配置

发布方

#! /usr/bin/env python
import rospy  # 导入 ROS 的 Python 库，用于节点创建、消息发布/订阅等
import tf2_ros  # 导入 tf2 库，用于发布和监听坐标变换
import tf  # 导入 tf 库，用于处理坐标变换中的四元数和欧拉角等
from turtlesim.msg import Pose  # 导入乌龟位姿消息类型（Pose），包含 x, y, theta
from geometry_msgs.msg import TransformStamped  # 导入 TransformStamped 消息类型，用于发布坐标变换'''发布方：订阅乌龟的位姿信息，转换成坐标系的相对关系，再发布准备：话题：/turtle1/pose类型：/turtlesim/Pose流程：1.导包2.初始化ros节点3.创建订阅对象4.回调函数处理订阅到的消息（核心）5.spin（）
'''def doPose(pose):# 1. 创建发布坐标系相对关系的对象pub = tf2_ros.TransformBroadcaster()  # 创建 tf2 的 TransformBroadcaster 对象，用于发布坐标变换# 2. 将 pose 转换成坐标系相对关系消息ts = TransformStamped()  # 创建 TransformStamped 对象，用于存储坐标变换ts.header.frame_id = "world"  # 设置父坐标系为 "world"ts.header.stamp = rospy.Time.now()  # 设置当前时间戳ts.child_frame_id = "turtle1"  # 设置子坐标系为 "turtle1"# 子级坐标系相对于父级坐标系的偏移量ts.transform.translation.x = pose.x  # 设置乌龟的 x 坐标ts.transform.translation.y = pose.y  # 设置乌龟的 y 坐标ts.transform.translation.z = 0  # 乌龟是 2D 的，所以 z 坐标设置为 0# 四元数# 从欧拉角转换为四元数'''乌龟是 2D 的，不存在 X 上的翻滚，Y 上的俯仰，只有 Z 上的偏航角（theta）0, 0, pose.theta'''qtn = tf.transformations.quaternion_from_euler(0, 0, pose.theta)  # 将偏航角转换为四元数ts.transform.rotation.x = qtn[0]  # 设置四元数的 x 分量ts.transform.rotation.y = qtn[1]  # 设置四元数的 y 分量ts.transform.rotation.z = qtn[2]  # 设置四元数的 z 分量ts.transform.rotation.w = qtn[3]  # 设置四元数的 w 分量# 3. 发布pub.sendTransform(ts)  # 发布变换信息if __name__ == "__main__":# 2. 初始化 ros 节点rospy.init_node("dynamic_pub_p")  # 初始化 ROS 节点，节点名为 "dynamic_pub_p"# 3. 创建订阅对象sub = rospy.Subscriber("/turtle1/pose", Pose, doPose, queue_size=100)  # 订阅 "/turtle1/pose" 话题，回调函数为 doPose# 4. 回调函数处理订阅到的消息（核心）# 5. spin()rospy.spin()  # 保持节点运行并调用回调函数

订阅方

#! /usr/bin/env python
import rospy  # 导入 ROS 的 Python 库，用于节点创建、消息发布/订阅等
import tf2_ros  # 导入 tf2 库，用于坐标变换的监听和发布
# 不要使用 geometry_msgs, 需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import tf2_geometry_msgs  # 导入 tf2_geometry_msgs 库，用于处理 tf2 坐标变换中的几何消息类型
# from geometry_msgs.msg import PointStamped  # （注释掉的）原本如果不使用 tf2 内置的消息类型时可以使用该库'''订阅方：订阅坐标变换消息，传入被转换的坐标点，调用转换算法流程：1.导包2.初始化3.创建订阅对象4.组织被转换的坐标点5.转换逻辑实现，调用 tf 封装的算法6.输出结果
'''if __name__ == "__main__":# 2. 初始化rospy.init_node("dynamic_sub_p")  # 初始化 ROS 节点，节点名为 "dynamic_sub_p"# 3. 创建订阅对象# 3-1. 创建缓存对象buffer = tf2_ros.Buffer()  # 创建 tf2 缓存对象，用于存储坐标变换信息# 3-2. 创建订阅对象（将缓存传入）sub = tf2_ros.TransformListener(buffer)  # 创建 TransformListener 对象，监听 tf2 坐标变换，并将其存入缓存# 4. 组织被转换的坐标点ps = tf2_geometry_msgs.PointStamped()  # 创建 PointStamped 消息对象，用于存储待转换的坐标点# 时间戳 -- 0ps.header.stamp = rospy.Time()  # 设置时间戳为当前时间# 坐标系ps.header.frame_id = "turtle1"  # 设置参考坐标系为 "turtle1"ps.point.x = 2.0  # 设置坐标点的 x 坐标ps.point.y = 3.0  # 设置坐标点的 y 坐标ps.point.z = 5.0  # 设置坐标点的 z 坐标# 5. 转换逻辑实现，调用 tf 封装的算法rate = rospy.Rate(1)  # 设置循环频率为 1 Hzwhile not rospy.is_shutdown():  # 当 ROS 节点没有被关闭时继续循环try:# 转换实现ps_out = buffer.transform(ps, "world")  # 将坐标点 ps 从 "turtle1" 坐标系转换到 "world" 坐标系# 6. 输出结果rospy.loginfo("转换后的坐标：（%.2f, %.2f, %.2f）,参考的坐标系：%s",ps_out.point.x, ps_out.point.y, ps_out.point.z, ps_out.header.frame_id)  # 输出转换后的坐标点except Exception as e:rospy.loginfo("错误提示！！！:%s", e)  # 如果发生异常，输出错误信息rate.sleep()  # 按照设定的频率休眠

2、多坐标变换

需求描述:

现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

实现分析:

首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
最后，还要实现坐标点的转换
实现流程:C++ 与 Python 实现流程一致

新建功能包，添加依赖

创建坐标相对关系发布方(需要发布两个坐标相对关系)

创建坐标相对关系订阅方

执行

准备工作

C++实现

发布方

创建launch文件发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息

订阅方

配置

编写cpp文件

#include "ros/ros.h"  // ROS基础功能头文件
#include "tf2_ros/transform_listener.h"  // TF2变换监听器头文件
#include "tf2/LinearMath/Quaternion.h"  // 四元数相关操作
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  // TF2与geometry_msgs转换
#include "geometry_msgs/TransformStamped.h"  // 变换消息类型
#include "geometry_msgs/PointStamped.h"  // 带时间戳的坐标点类型/*订阅方实现功能：1.计算 son1 与 son2 的相对关系 2.计算 son1 中的某个坐标点在 son2 中的坐标值流程：1.包含头文件2.编码 初始化 NodeHandle3.创建订阅对象4.编写解析逻辑5.spinOnce()
*/int main(int argc, char *argv[])
{// 2.初始化ROS节点和NodeHandlesetlocale(LC_ALL,"");  // 设置本地化，支持中文输出ros::init(argc,argv,"tfs_sub");  // 初始化节点，节点名为"tfs_sub"ros::NodeHandle nh;  // 创建节点句柄// 3.创建TF2缓冲区和监听器tf2_ros::Buffer buffer;  // 创建TF2缓冲区，用于存储变换数据tf2_ros::TransformListener sub(buffer);  // 创建TF2监听器，订阅/tf话题并将数据存入缓冲区// 4.创建并初始化一个在son1坐标系中的点geometry_msgs::PointStamped psAtSon1;  // 创建一个带时间戳的坐标点psAtSon1.header.stamp = ros::Time::now();  // 设置时间戳为当前时间psAtSon1.header.frame_id = "son1";  // 设置坐标系为son1psAtSon1.point.x = 1.0;  // 设置x坐标psAtSon1.point.y = 2.0;  // 设置y坐标psAtSon1.point.z = 3.0;  // 设置z坐标ros::Rate rate(1);  // 设置循环频率为1Hzwhile (ros::ok())  // 主循环{try{// 功能1：计算son1相对于son2的坐标变换/*lookupTransform函数参数说明：参数1：目标坐标系 (son2)参数2：源坐标系 (son1)参数3：ros::Time(0) 表示获取最新的可用变换返回值：geometry_msgs::TransformStamped 源坐标系相对于目标坐标系的变换*/geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2","son1",ros::Time(0));ROS_INFO("son1 相对于 son2 的信息：父级：%s，子级：%s  偏移量（%.2f,%.2f,%.2f）",son1ToSon2.header.frame_id.c_str(),    // 父坐标系(son2)son1ToSon2.child_frame_id.c_str(),     // 子坐标系(son1)son1ToSon2.transform.translation.x,    // x方向偏移son1ToSon2.transform.translation.y,    // y方向偏移son1ToSon2.transform.translation.z);   // z方向偏移// 功能2：将son1中的点转换到son2坐标系/*transform函数参数说明：参数1：要转换的点(psAtSon1)参数2：目标坐标系(son2)返回值：在目标坐标系中的点坐标*/geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2");ROS_INFO("坐标点在 Son2 中的值(%.2f,%.2f,%.2f)",psAtSon2.point.x,  // 转换后的x坐标psAtSon2.point.y,  // 转换后的y坐标psAtSon2.point.z); // 转换后的z坐标}catch(const std::exception& e)  // 捕获异常{ROS_INFO("错误提示：%s ！！！！！",e.what());  // 打印错误信息}rate.sleep();  // 控制循环频率ros::spinOnce();  // 处理回调函数}return 0;
}

编译启动roscore 执行

Python实现

配置添加可执行权限

编写py文件

#!/usr/bin/env python
"""  需求:现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标实现流程:   1.导包2.初始化 ROS 节点3.创建 TF 订阅对象4.调用 API 求出 son1 相对于 son2 的坐标关系5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标6.spin
"""
# 1.导包
import rospy  # ROS Python接口
import tf2_ros  # TF2 Python接口
from geometry_msgs.msg import TransformStamped  # 变换消息类型
from tf2_geometry_msgs import PointStamped  # 带时间戳的坐标点类型if __name__ == "__main__":# 2.初始化 ROS 节点rospy.init_node("frames_sub_p")  # 创建节点，节点名为"frames_sub_p"# 3.创建 TF 订阅对象buffer = tf2_ros.Buffer()  # 创建TF2缓冲区，存储变换数据listener = tf2_ros.TransformListener(buffer)  # 创建监听器，订阅/tf话题rate = rospy.Rate(1)  # 设置循环频率为1Hzwhile not rospy.is_shutdown():  # 主循环try:# 4.调用API获取son1相对于son2的坐标关系# lookup_transform参数说明：#   target_frame: 目标坐标系(son2)#   source_frame: 源坐标系(son1)#   time: rospy.Time(0)表示获取最新可用变换tfs = buffer.lookup_transform("son2","son1",rospy.Time(0))rospy.loginfo("son1 与 son2 相对关系:")rospy.loginfo("父级坐标系:%s",tfs.header.frame_id)  # 父坐标系(son2)rospy.loginfo("子级坐标系:%s",tfs.child_frame_id)  # 子坐标系(son1)rospy.loginfo("相对坐标:x=%.2f, y=%.2f, z=%.2f",tfs.transform.translation.x,  # x方向偏移tfs.transform.translation.y,  # y方向偏移tfs.transform.translation.z,  # z方向偏移)# 5.创建son1中的点并转换到son2坐标系point_source = PointStamped()  # 创建源坐标点point_source.header.frame_id = "son1"  # 设置坐标系为son1point_source.header.stamp = rospy.Time.now()  # 设置当前时间戳point_source.point.x = 1  # 设置x坐标point_source.point.y = 1  # 设置y坐标point_source.point.z = 1  # 设置z坐标# transform参数说明：#   point_source: 要转换的点#   target_frame: 目标坐标系(son2)#   timeout: 超时时间(0.5秒)point_target = buffer.transform(point_source,"son2",rospy.Duration(0.5))rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id)  # 转换后的坐标系rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)",point_target.point.x,  # 转换后的x坐标point_target.point.y,  # 转换后的y坐标point_target.point.z  # 转换后的z坐标)except Exception as e:  # 捕获异常rospy.logerr("错误提示:%s",e)  # 打印错误信息rate.sleep()  # 控制循环频率# 6.spin    # rospy.spin()