ros0基础-day18

1、TF坐标变换实操
需求描述:

程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

实现分析:

乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

启动乌龟显示节点
在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
编写两只乌龟发布坐标信息的节点
编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息
实现流程:C++ 与 Python 实现流程一致

新建功能包，添加依赖

编写服务客户端，用于生成一只新的乌龟

编写发布方，发布两只乌龟的坐标信息

编写订阅方，订阅两只乌龟信息，生成速度信息并发布

运行

准备工作:

1.了解如何创建第二只乌龟，且不受键盘控制

创建第二只乌龟需要使用rosservice,话题使用的是 spawn

键盘是无法控制第二只乌龟运动的，因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息

2.了解如何获取两只乌龟的坐标

是通过话题 /乌龟名称/pose 来获取的

C++实现

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

服务客户端(生成乌龟)

​
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*
需求：是向服务端发送请求，生成一只新乌龟话题：/spawn消息：turtlesim/Spawn1、包含头文件2、初始化ROS节点3、创建节点句柄4、创建客户端对象5、组织数据并且发送6、处理响应（数据什么时候发送，客户端是可控的，不需要回调函数处理）
*/int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2、初始化ROS节点ros::init(argc,argv,"service_call");// 3、创建节点句柄ros::NodeHandle nh;// 4、创建客户端对象ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");// 5、组织数据并且发送//组织请求数据turtlesim::Spawn spawn;spawn.request.x = 1.3;spawn.request.y = 3.3;spawn.request.theta = 1.75;spawn.request.name = "turtle2";//发送请求//ros::service::waitForService("/spawn");client.waitForExistence();bool flag = client.call(spawn);   //flag用来接收响应状态，响应结果也会被设置进spawn对象// 6、处理响应if (flag){ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());}else{ROS_INFO("请求失败，不生成小乌龟");}return 0;
}​

发布方(发布两只乌龟的坐标信息)

可以订阅乌龟的位姿信息，然后再转换成坐标信息，两只乌龟的实现逻辑相同，只是订阅的话题名称，生成的坐标信息等稍有差异，可以将差异部分通过参数传入:

• 该节点需要启动两次
• 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

#include "ros/ros.h"  // 引入ROS核心库，提供ROS节点、通信等功能
#include "turtlesim/Pose.h"  // 引入乌龟位姿消息类型Pose，用于获取乌龟的位置和方向
#include "tf2_ros/transform_broadcaster.h"  // 引入tf2库中的TransformBroadcaster，用于发布坐标变换
#include "geometry_msgs/TransformStamped.h"  // 引入变换数据类型TransformStamped，存储坐标变换的信息
#include "tf2/LinearMath/Quaternion.h"  // 引入tf2库中的四元数类Quaternion，用于表示旋转/*发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布准 备：话题：/turtle1/pose  // 订阅的主题是乌龟1的位姿，包含位置（x, y）和方向（theta）消息：/turtlesim/Pose  // 消息类型为 Pose 类型，包含乌龟的位置和角度流程：1.包含头文件2.设置编码、初始化、NodeHandle3.创建订阅对象，订阅 /turtle1/pose4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布（关注）5.spin()  // ROS的事件循环，使节点保持活跃并等待回调
*/// 声明变量接收传递的参数
std::string turtle_name;  // 用于保存动态传入的乌龟名字（turtle1 或 turtle2）// 回调函数：处理收到的乌龟位姿信息
void doPose(const turtlesim::Pose::ConstPtr& pose)
{// 获取位姿信息，转换成坐标相对关系（核心），并发布//a.创建发布对象，广播变换static tf2_ros::TransformBroadcaster pub;  // tf2_ros::TransformBroadcaster 用于广播坐标变换//b.组织被发布的数据geometry_msgs::TransformStamped ts;  // 变换信息ts.header.frame_id = "world";  // 设置父坐标系为世界坐标系（"world"）ts.header.stamp = ros::Time::now();  // 设置时间戳为当前时间// 关键点2：动态传入的子坐标系名称（turtle_name），即乌龟的名称（turtle1 或 turtle2）ts.child_frame_id = turtle_name;  // 设置子坐标系的名称为乌龟的名字（turtle1 或 turtle2）// 设置坐标系偏移量，坐标信息来自于收到的位姿消息ts.transform.translation.x = pose->x;  // x 轴位置ts.transform.translation.y = pose->y;  // y 轴位置ts.transform.translation.z = 0;  // z 轴位置为0，2D空间// 坐标系四元数的设置/*位姿信息中没有四元数，但是有个偏航角度（theta），乌龟是2D空间（没有翻滚与俯仰角度），所以可以得出乌龟的欧拉角：0，0，theta*/tf2::Quaternion qtn;  // 创建四元数对象qtn.setRPY(0, 0, pose->theta);  // 设置四元数的偏航角为pose->theta（由位姿信息提供）// 将四元数的值存入变换消息中ts.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_pub"ros::NodeHandle nh;  // 创建NodeHandle对象，用于与ROS系统通信/*解析 launch 文件通过 args 传入的参数*/if (argc != 2)  // 检查是否传入了一个参数（乌龟的名字）{ROS_ERROR("请传入一个参数");  // 如果参数不足，则输出错误信息return 1;  // 返回1，表示错误} else {turtle_name = argv[1];  // 获取传入的乌龟名称（如turtle1或turtle2）}// 3.创建订阅对象，订阅 /turtle1/pose// 关键点1：订阅的话题名称动态传入，如turtle1/pose 或 turtle2/poseros::Subscriber sub = nh.subscribe(turtle_name + "/pose", 100, doPose);  // 订阅话题，调用doPose回调函数// 4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布// 5.spin()：持续等待回调处理，ROS的事件循环机制ros::spin();  // 进入事件循环，等待消息到达并处理return 0;  // 程序正常退出
}

订阅方(解析坐标信息并生成速度信息)

#include "ros/ros.h"  // 引入ROS核心库，提供节点、通信、日志等功能
#include "tf2_ros/transform_listener.h"  // 引入tf2库中的TransformListener，用于监听坐标变换
#include "tf2/LinearMath/Quaternion.h"  // 引入tf2库中的四元数类Quaternion，用于表示旋转
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  // 引入tf2_geometry_msgs，提供ROS消息与tf2的兼容
#include "geometry_msgs/TransformStamped.h"  // 引入TransformStamped消息类型，用于表示坐标变换
#include "geometry_msgs/PointStamped.h"  // 引入PointStamped消息类型，用于表示带时间戳的点
#include "geometry_msgs/Twist.h"  // 引入Twist消息类型，用于表示速度（线速度与角速度）/*需求1：换算出 turtle1 相对于 turtle2 的关系需求2：计算角速度和线速度并发布
*/int main(int argc, char *argv[])
{// 2.编码 初始化 NodeHandlesetlocale(LC_ALL,"");  // 设置本地化信息ros::init(argc, argv, "tfs_sub");  // 初始化ROS节点，节点名称为"tfs_sub"ros::NodeHandle nh;  // 创建NodeHandle对象，用于与ROS系统通信// 3.创建订阅对象tf2_ros::Buffer buffer;  // 创建一个tf2缓存对象，用于存储坐标变换数据tf2_ros::TransformListener sub(buffer);  // 创建tf2的监听器，监听坐标变换数据// A 创建发布对象ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 100);  // 创建发布对象，发布到"/turtle2/cmd_vel"话题，消息类型为Twist（线速度与角速度）ros::Rate rate(10);  // 设置循环频率为10Hzwhile (ros::ok())  // 循环直到ROS系统关闭{// 核心try{// 1.计算 turtle1 与 turtle2 的相对关系/*A 相对于 B 的坐标系关系参数1：目标坐标系 B ("turtle2")参数2：源坐标系 A ("turtle1")参数3：ros::Time(0) 取间隔最短的两个坐标关系帧计算相对关系返回值：geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系*/geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2", "turtle1", ros::Time(0));  // 获取turtle1相对于turtle2的坐标变换// 2. 根据相对关系计算并组织速度geometry_msgs::Twist twist;  // 创建Twist消息，用于存储计算出的速度信息/* 组织速度，只需要设置线速度的 x 与角速度的 zx = 系数 * 开方(y^2 + x^2)  // 线速度的x分量，即从turtle1到turtle2的距离z = 系数 * arctan(对边，邻边)  // 角速度的z分量，计算turtle1相对于turtle2的方向角*/twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x, 2) + pow(son1ToSon2.transform.translation.y, 2));  // 计算线速度twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y, son1ToSon2.transform.translation.x);  // 计算角速度// C 发布pub.publish(twist);  // 发布计算出的速度信息到"/turtle2/cmd_vel"}catch (const std::exception& e){ROS_INFO("错误提示：%s ！！！！！", e.what());  // 捕捉异常并输出错误信息}rate.sleep();  // 以设定频率（10Hz）暂停ros::spinOnce();  // 处理ROS消息队列中的回调函数}// 5.spinOnce()：此函数调用确保ROS消息循环继续进行return 0;  // 程序正常退出
}

运行
使用 launch 文件组织需要运行的节点，内容示例如下:

<launch><!-- 1. 启动乌龟GUI节点 --><node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" /><node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" /><!-- 2. 生成新的乌龟GUI节点 --><node pkg="tf04_test" type="test01_new_turtle" name="turtle2" output="screen" /><!-- 3.需要启动两个乌龟相对于世界的坐标关系的发布 --><!--基本实现思路：1.节点只编写一个2.这个节点需要启动两次3.节点启动时动态传参；第一次启动传递turtle1，第二次启动传递turtle2--><node pkg="tf04_test" type="test02_pub_turtle" name="pub1" args="turtle1" output="screen" /><node pkg="tf04_test" type="test02_pub_turtle" name="pub2" args="turtle2" output="screen" /><!-- 4.需要订阅 turtle1 与 turtle2 相对于世界坐标系的坐标消息 ，并转换成 turtle1 相对于 turtle2 的坐标消息，再生成速度消息--><node pkg="tf04_test" type="test03_control_turtle2" name="control" output="screen" /></launch>

Python实现

准备工作

生成新的小乌龟

#! /usr/bin/env python
import rospy  # 引入ROS Python客户端库，用于初始化ROS节点和进行通信
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse  # 引入turtlesim中的Spawn服务和请求、响应消息类型"""
需求：向服务器发送请求生成一只小乌龟
话题：/spawn
消息：turtlesim/Spawn1.导包2.初始化ros节点3、创建服务的客户端对象4、组织数据并发布请求5、处理响应结果
"""if __name__ == "__main__":# 2.初始化ros节点rospy.init_node("service_call_p")  # 初始化ROS节点，节点名称为"service_call_p"# 3、创建服务的客户端对象client = rospy.ServiceProxy("/spawn", Spawn)  # 创建一个客户端对象client，连接到名为"/spawn"的服务，消息类型为Spawn# 4、组织数据并发布请求# 组织数据：创建一个SpawnRequest请求对象并填充数据request = SpawnRequest()  # 创建一个SpawnRequest对象，用于组织发送给服务端的请求数据request.x = 2.3  # 设置乌龟的x坐标为2.3request.y = 4.3  # 设置乌龟的y坐标为4.3request.theta = -3  # 设置乌龟的方向角度theta为-3request.name = "turtle2"  # 设置新生成的乌龟的名字为"turtle2"# 判断服务是否可用再发送请求client.wait_for_service()  # 等待"/spawn"服务可用，如果服务未启动，程序将一直阻塞直到服务启动try:# 发送请求并获取响应response = client.call(request)  # 调用client的call方法，向"/spawn"服务发送请求，返回响应结果# 5、处理响应结果rospy.loginfo("生成乌龟的名字叫：%s", response.name)  # 打印生成的乌龟的名字，即响应中的nameexcept Exception as e:rospy.loginfo("请求处理异常")  # 如果请求过程中出现异常，捕获异常并输出错误信息

发布两只小乌龟坐标信息

#! /usr/bin/env python
import rospy  # 导入ROS的Python客户端库，提供ROS节点和通信功能
import tf2_ros  # 导入tf2_ros，用于处理坐标变换广播
import tf  # 导入tf库，用于欧拉角和四元数之间的转换
from turtlesim.msg import Pose  # 导入Pose消息类型，表示乌龟的位姿信息
from geometry_msgs.msg import TransformStamped  # 导入TransformStamped消息类型，用于发布坐标变换
import sys  # 导入sys库，用于读取命令行参数
'''发布方：订阅乌龟的位姿信息，转换成坐标系的相对关系，再发布准 备：话题：/turtle1/pose类型：/turtlesim/Pose流程：1.导包2.初始化ros节点3.创建订阅对象4.回调函数处理订阅到的消息（核心）5.spin（）
'''
# 接收乌龟名称变量
turtle_name = ""  # 初始化turtle_name变量，用于存储动态传入的乌龟名称def doPose(pose):#1. 创建发布坐标系相对关系的对象pub = tf2_ros.TransformBroadcaster()  # 创建一个TransformBroadcaster对象，用于发布坐标变换#2. 将pose转换成坐标系相对关系消息ts = TransformStamped()  # 创建一个TransformStamped对象，表示一个时间戳的坐标变换ts.header.frame_id = "world"  # 设置父坐标系为"world"ts.header.stamp = rospy.Time.now()  # 设置当前时间戳# 修改2：子级坐标系名称ts.child_frame_id = turtle_name  # 子坐标系的名称是动态传入的乌龟名称# 子级坐标系相对于父级坐标系的偏移量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轴上的偏航所以我们只关心Z轴的旋转，欧拉角(0, 0, pose.theta)'''qtn = tf.transformations.quaternion_from_euler(0, 0, pose.theta)  # 使用tf库将欧拉角转为四元数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)  # 使用TransformBroadcaster发布坐标变换if __name__ == "__main__":# 2. 初始化ros节点rospy.init_node("dynamic_pub_p")  # 初始化ROS节点，节点名称为"dynamic_pub_p"# 解析传入的参数，确保有正确的命令行参数if len(sys.argv) != 4:  # 如果传入的参数个数不为4（包括脚本文件路径、乌龟名称、节点名称和日志路径）rospy.loginfo("传入的参数不对！！！")  # 打印错误信息sys.exit(1)  # 退出程序，返回状态码1，表示错误else:turtle_name = sys.argv[1]  # 从命令行参数中获取乌龟的名称，赋值给turtle_name变量# 3. 创建订阅对象# 修改1：订阅的主题名称是动态传入的乌龟名称，构建话题名称"/turtle_name/pose"sub = rospy.Subscriber(turtle_name + "/pose", Pose, doPose, queue_size=100)  # 订阅话题，接收到Pose消息时调用doPose回调函数# 4. 回调函数处理订阅到的消息（核心）# 5. spin（）rospy.spin()  # 进入ROS事件循环，等待回调函数触发

解析坐标信息并生成速度信息

#! /usr/bin/env python
"""  订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布实现流程:1.导包2.初始化 ros 节点3.创建 TF 订阅对象4.处理订阅到的 TF4-1.查找坐标系的相对关系4-2.生成速度信息，然后发布
"""
# 1.导包
import rospy  # 导入ROS的Python客户端库，提供ROS节点和通信功能
import tf2_ros  # 导入tf2_ros，用于处理坐标变换的监听和广播
from geometry_msgs.msg import TransformStamped, Twist  # 导入TransformStamped和Twist消息类型
import math  # 导入math库，用于数学计算if __name__ == "__main__":# 2.初始化 ros 节点rospy.init_node("sub_tfs_p")  # 初始化一个ROS节点，节点名称为"sub_tfs_p"# 3.创建 TF 订阅对象buffer = tf2_ros.Buffer()  # 创建一个TF变换缓存对象，用于缓存TF变换listener = tf2_ros.TransformListener(buffer)  # 创建一个TransformListener对象，监听TF变换并存入缓存# 4.处理订阅到的 TFrate = rospy.Rate(10)  # 设置循环频率为10Hz# 创建速度发布对象，发布到话题"/turtle2/cmd_vel"，类型为Twistpub = rospy.Publisher("/turtle2/cmd_vel", Twist, queue_size=1000)  # 创建一个发布对象，发布线速度和角速度while not rospy.is_shutdown():  # 在ROS节点运行期间持续循环rate.sleep()  # 按照设定的频率休眠try:# 查找turtle2相对于turtle1的坐标变换（时间为0表示查找最接近的时间点的TF变换）trans = buffer.lookup_transform("turtle2", "turtle1", rospy.Time(0))# 根据转变后的坐标计算出速度和角速度信息twist = Twist()  # 创建一个Twist消息对象，用于表示线速度和角速度# 计算线速度：两点间的距离twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x, 2) + math.pow(trans.transform.translation.y, 2))# 计算角速度：使用atan2计算turtle1相对于turtle2的角度twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)pub.publish(twist)  # 发布计算出的速度信息到"/turtle2/cmd_vel"话题except Exception as e:  # 如果发生异常，则输出警告信息rospy.logwarn("警告:%s", e)

执行

<launch><!-- 流程详解：1.准备工作：启动乌龟 GUI 节点与键盘控制节点；2.需要调用服务生成一只新的乌龟3.发布两只乌龟的坐标信息4.订阅坐标信息，并且转换成 乌龟A 相对于 乌龟B 的坐标信息，最后再生成控制乌龟B的速度信息--><!-- 1. 启动乌龟GUI节点 --><node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" /><node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" /><!-- 2. 生成新的乌龟GUI节点 --><node pkg="tf04_test" type="test01_new_turtle_p.py" name="turtle2" output="screen" /><!-- 3.需要启动两个乌龟相对于世界的坐标关系的发布基本实现思路：1.节点只编写一个2.这个节点需要启动两次3.节点启动时动态传参；第一次启动传递turtle1，第二次启动传递turtle21.复用之前的乌龟坐标发布功能2.调用节点时，以参数的方式传递乌龟名称，解析参数置换：订阅的话题消息 和 子级坐标系的名称--><node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub1" args="turtle1" output="screen" /><node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub2" args="turtle2" output="screen" /><!-- 4.需要订阅 turtle1 与 turtle2 相对于世界坐标系的坐标消息 ，并转换成 turtle1 相对于 turtle2 的坐标消息，再生成速度消息--><node pkg="tf04_test" type="test03_control_turtle2_p.py" name="control" output="screen" /></launch>

坐标变换在机器人系统中是一个极其重要的组成模块，在 ROS 中 TF2 组件是专门用于实现坐标变换的，TF2 实现具体内容又主要介绍了如下几部分:

1.静态坐标变换广播器，可以编码方式或调用内置功能包来实现(建议后者)，适用于相对固定的坐标系关系

2.动态坐标变换广播器，以编码的方式广播坐标系之间的相对关系，适用于易变的坐标系关系

3.坐标变换监听器，用于监听广播器广播的坐标系消息，可以实现不同坐标系之间或同一点在不同坐标系之间的变换

4.机器人系统中的坐标系关系是较为复杂的，还可以通过 tf2_tools 工具包来生成 ros 中的坐标系关系图

2、rosbag

机器人传感器获取到的信息，有时我们可能需要时时处理，有时可能只是采集数据，事后分析，比如:

机器人导航实现中，可能需要绘制导航所需的全局地图，地图绘制实现，有两种方式，方式1：可以控制机器人运动，将机器人传感器感知到的数据时时处理，生成地图信息。方式2：同样是控制机器人运动，将机器人传感器感知到的数据留存，事后，再重新读取数据，生成地图信息。两种方式比较，显然方式2使用上更为灵活方便。

在ROS中关于数据的留存以及读取实现，提供了专门的工具: rosbag。

概念

是用于录制和回放 ROS 主题的一个工具集。

作用

实现了数据的复用，方便调试、测试。

本质

rosbag本质也是ros的节点，当录制时，rosbag是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当重放时，rosbag是一个发布节点，可以读取磁盘文件，发布文件中的话题消息。

 rosbag使用_命令行

需求:

ROS 内置的乌龟案例并操作，操作过程中使用 rosbag 录制，录制结束后，实现重放

实现:

1.准备

创建目录保存录制的文件

mkdir -p ./bags

2.开始录制

rosbag record -a -o 目标文件

操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成bag文件。

3.查看文件

rosbag info 文件名

4.回放文件

rosbag play 文件名

重启乌龟节点，会发现，乌龟按照录制时的轨迹运动。

编码方式使用rosbag

简单读写演示

方案A:C++实现

写bag：

配置

编写cpp文件

#include "ros/ros.h"
#include "rosbag/bag.h"
#include "std_msgs/String.h"/*需求：使用 rosbag 向磁盘文件写出数据（话题 + 消息）流程：1.导包2.初始化3.创建 rosbag 对象4.打开文件流5.写数据6.关闭文件流
*/
int main(int argc, char *argv[])
{// 2.初始化setlocale(LC_ALL,"");  // 设置本地化，确保字符编码处理正确（尤其是非英文字符）ros::init(argc,argv,"bag_write");  // 初始化ROS节点，节点名为"bag_write"ros::NodeHandle nh;  // 创建节点句柄，负责与ROS系统通信// 3.创建 rosbag 对象rosbag::Bag bag;  // 创建一个rosbag对象，用于操作ROS数据包// 4.打开文件流bag.open("hello.bag",rosbag::BagMode::Write);  // 打开一个名为"hello.bag"的rosbag文件，模式为写入模式// 5.写数据std_msgs::String msg;  // 创建一个标准的ROS消息对象，用于发送字符串msg.data = "hello xiaoxiao";  // 为消息对象赋值，数据内容为"hello xiaoxiao"// 将消息写入到rosbag文件中bag.write("/chatter", ros::Time::now(), msg);  // 将消息msg写入到"/chatter"话题，使用当前时间戳bag.write("/chatter", ros::Time::now(), msg);  // 再写入一次bag.write("/chatter", ros::Time::now(), msg);  // 再写入一次/*参数1:话题参数2:时间戳参数3:消息*/// 6.关闭文件流bag.close();  // 关闭rosbag文件，确保数据被保存return 0;  // 程序结束
}

编译 启动roscore 执行

读bag：

配置

编写cpp文件

#include "ros/ros.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"/*需求：使用 rosbag 读取磁盘文件数据（话题 + 消息）流程：1.导包2.初始化3.创建 rosbag 对象4.打开文件流(读方式打开)5.读数据6.关闭文件流
*/int main(int argc, char *argv[])
{// 2.初始化setlocale(LC_ALL,"");  // 设置本地化，确保字符编码处理正确（尤其是非英文字符）ros::init(argc, argv, "bag_read");  // 初始化ROS节点，节点名为"bag_read"ros::NodeHandle nh;  // 创建节点句柄，负责与ROS系统通信// 3.创建 rosbag 对象rosbag::Bag bag;  // 创建一个rosbag对象，用于操作ROS数据包// 4.打开文件流(读方式打开)bag.open("hello.bag", rosbag::BagMode::Read);  // 打开名为"hello.bag"的rosbag文件，模式为读取模式// 5.读数据// 取出话题 时间戳 消息内容// 可以先获取消息的集合，再迭代取出消息的字段for (auto &&m : rosbag::View(bag))  // 使用for循环遍历rosbag文件中的每一条消息{// 解析每条消息std::string topic = m.getTopic();  // 获取消息的发布话题名称ros::Time time = m.getTime();  // 获取消息的时间戳std_msgs::String::ConstPtr p = m.instantiate<std_msgs::String>();  // 将消息转换为std_msgs::String类型的指针// 判断消息是否成功转换if (p != nullptr){ROS_INFO("读取的数据,话题:%s,时间戳:%.2f,消息值:%s", topic.c_str(), time.toSec(), p->data.c_str()); // 如果成功，输出话题、时间戳和消息的内容}}// 6.关闭文件流bag.close();  // 关闭rosbag文件，确保数据处理完成return 0;  // 程序结束
}

编译 启动roscore 执行

方案B：Python实现

写bag：

配置，添加可执行权限

编写py文件

#! /usr/bin/env python
import rospy
import rosbag
from std_msgs.msg import String'''需求:写出消息数据到磁盘上的 bag 文件流程:1.导包2.初始化3.创建 rosbag 对象并且打开文件流4.写数据5.关闭文件流
'''if __name__ == "__main__":# 2.初始化rospy.init_node("write_bag_p")  # 初始化ROS节点，节点名为"write_bag_p"# 3.创建 rosbag 对象并且打开文件流bag = rosbag.Bag("hello_p.bag", "w")  # 创建一个rosbag对象，打开"hello_p.bag"文件，使用写入模式("w")# 4.写数据msg = String()  # 创建一个std_msgs/String类型的消息对象msg.data = "hello bag!!!!!!!!!!!"  # 设置消息内容# 写入数据到bag文件bag.write("/liaoTian", msg)  # 写入消息到话题"/liaoTian"bag.write("/liaoTian", msg)  # 再次写入相同消息到话题"/liaoTian"bag.write("/liaoTian", msg)  # 再次写入相同消息到话题"/liaoTian"# 5.关闭文件流bag.close()  # 关闭rosbag文件，确保所有数据都被写入

编译 启动roscore 执行

读bag：

配置，添加可执行权限

编写py文件

#! /usr/bin/env python
import rospy
import rosbag
from std_msgs.msg import String'''需求:读取磁盘上的 bag 文件流程:1.导包2.初始化3.创建 rosbag 对象并且打开文件流4.读数据5.关闭文件流
'''if __name__ == "__main__":# 2.初始化rospy.init_node("read_bag_p")  # 初始化ROS节点，节点名为"read_bag_p"# 3.创建 rosbag 对象并且打开文件流bag = rosbag.Bag("hello_p.bag", 'r')  # 创建一个rosbag对象，打开"hello_p.bag"文件，使用读取模式("r")# 4.读数据msgs = bag.read_messages("/liaoTian")  # 从bag文件中读取指定话题"/liaoTian"的消息for topic, msg, time in msgs:  # 遍历读取的消息集合rospy.loginfo("话题:%s,消息:%s,时间:%s", topic, msg.data, time)  # 打印话题、消息数据和时间戳# 5.关闭文件流bag.close()  # 关闭rosbag文件

编译 启动roscore 执行