Rosbag数据解析实例
文章目录
前言
今天写代码出了bug，解问题花了点时间。本来打算介绍一下基于bag数据，解析数据并通过rviz可视化的，现在是晚上8点，准备下班拉，只能回家或者明天上午早点写，希望大家原谅。
前几篇文章大家可以回看一下。
一、rosbag相关指令
rosbag常用命令
rosbag check 检查一个包是否可以在当前系统中播放，是否需要迁移
Bag file does not need any migrations.表示不需要迁移
rosbag compress 压缩一个或多个bag文件
rosbag compress命令用于压缩ROS bag文件，它提供了两种压缩格式：BZ2和LZ4。BZ2压缩格式虽然占用的硬盘空间较小，但播放速度较慢；而LZ4压缩格式虽然对体积的压缩效果不明显，但播放速度有显著提高。
rosbag decompress 解压缩一个或多个bag文件
解压缩一个或多个bag文件
每次解压缩文件之前，会备份每个包文件（扩展名为 .orig.bag），如果备份文件已存在（并且未指定 -f 选项），则该工具将不会解压缩该文件
rosbag filter 根据条件过滤bag文件内容
根据条件过滤bag文件内容，显示与指定python语法的逻辑表达式匹配的消息
rosbag filter input.bag output.bag "逻辑表达式"
rosbag filter input.bag output.bag "m.data=='foo'"
其中，input.bag表示过滤之前的bag文件，output.bag表示过滤之后的bag文件。
逻辑表达式中的 m 表示 msg，另外，还有 topic 表示 topic，t 表示 timestamp
例如
rosbag filter big.bag small.bag "topic == '/chatter'"
rosbag fix 修复bag文件中的消息，以便在当前系统中播放
如前rosbag check 所述，如果bag文件需要迁移，则可以使用 rosbag fix 修复。
rosbag fix old.bag repaired.bag myrules.bmr
其中，old.bag为修复之前的bag文件，repaired.bag 为修复之后的bag文件，myrules.bmr为修复规则，修复规则相关详见 包迁移机制 。
rosbag info 查看bag文件简要信息
例如：
桌面$ rosbag info 20240805114530.bag
path: 20240805114530.bag
version: 2.0
duration: 30.0s
start: Aug 05 2024 11:45:10.69 (1722829510.69)
end: Aug 05 2024 11:45:40.66 (1722829540.66)
size: 257.6 MB
messages: 8742
compression: none [280/280 chunks]
types: autopilot_msgs/BinaryData [fc52aedbbd8b2b2e49bf64cab07160d9]
std_msgs/String [992ce8a1687cec8c8bd883ec73ca41d1]
visualization_msgs/Marker [4048c9de2a16f4ae8e0538085ebf1b97]
visualization_msgs/MarkerArray [d155b9ce5188fbaf89745847fd5882d7]
topics: /chassis/vehicle_state 3000 msgs : autopilot_msgs/BinaryData
/hadmap_engine/map_msg 300 msgs : autopilot_msgs/BinaryData
/localization/global 2999 msgs : autopilot_msgs/BinaryData
/perception/camera/trfclts_state 518 msgs : std_msgs/String
rosbag play 以时间同步的方式播放一个或多个bag文件的内容
以时间同步的方式播放一个或多个bag文件的内容，在播放时可以随时按 空格键 以暂停播放，同时暂停播放后，可以按 s 键以单步播放。
rosbag record 记录一个包含指定topic内容的bag文件
要记录指定话题的数据，可以使用以下命令：
rosbag record -O <bag_file_name> <topic_name>
其中，<bag_file_name> 是要保存的 bag 文件的名称，<topic_name> 是要记录的话题名称。例如，要记录名为 /sensor_data 的话题数据，并将其保存为 sensor_data.bag 文件，可以使用以下命令：
rosbag record -O sensor_data.bag /sensor_data
rosbag reindex 重新索引一个或多个bag文件
用于修复损坏的包文件（或 ROS 版本 0.11 之前记录的包文件）。如果由于任何原因未完全关闭包，则索引信息可能会损坏。使用该工具重新读取消息数据并重建索引。
在重新索引包之前，会备份每个包文件（扩展名为.orig.bag ）。如果备份文件已存在（并且未指定-f选项），则该工具将不会重新索引该文件。
1.ros的xx.msg、xx.srv、xx.proto编译
2.CMake介绍及CMakeLists.txt文件编写
3.RTK定位原理和数据解析
堆和栈的区别——vector引发的思考
圆曲线拟合和多项式拟合
二、rosbag数据解析及可视化
1.新建节点
节点名称为demo:
ros::init(argc, argv, "demo");
2.构造订阅器：
ros::Subscriber perceptionSub_ = n.subscribe("/perception/obstacles", 1, obstaclesCallback);
3.设计回调函数，并打印相关信息：
void obstaclesCallback(const BinaryDataConstPtr &msg) {
auto t1 = std::chrono::high_resolution_clock::now();
ROS_INFO("obstaclesCallback");
perception_index++;
static double total_time = 0.0;
perception::Objects objects;
rosToProto(*msg, objects);
double obj_time = objects.header().stamp().sec() + objects.header().stamp().nsec() * 1e-9;
int objs_num = objects.objs_size();
updateObstacles(&objects);
auto t2 = std::chrono::high_resolution_clock::now();
double cost_time = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1.0e9;
total_time += cost_time;
ROS_INFO(
"Index: %ld, obj_time: %.3lf, obj_num: %d, cost_time: %.6lf, average_time: %.6lf",
perception_index, obj_time, objs_num, cost_time, total_time / perception_index);
}
4.存储障碍物信息
void updateObstacles(perception::Objects *objects) {
auto t1 = std::chrono::high_resolution_clock::now();
obstacles_container_.append(objects);
auto t2 = std::chrono::high_resolution_clock::now();
auto timecost =
std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1.0e9;
ROS_INFO("updateObstacles timecost: %.6lf", timecost);
}
5.可视化障碍物信息
void Visualization::drawTrajectory(ObstaclesContainer &obstacles_container, double current_time,
GlobalToLocal &global_to_local,
ROS_PUBLISHER(ROS_MSGS(visualization_msgs, MarkerArray)) &
obj_trajectory) {
ROS_MSGS(visualization_msgs, MarkerArray) marker_array;
int marker_id = 0;
static int last_marker_size = 0;
ROS_MSGS(visualization_msgs, Marker) marker;
marker.action = marker.ADD;
marker.header.stamp = ros::Time::now();
marker.header.frame_id = "base_link";
marker.ns = "obj_trajectory";
marker.type = marker.LINE_STRIP;
// marker.type = ROS_MSGS(visualization_msgs, Marker)::SPHERE;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.4;
marker.scale.y = 0.4;
marker.scale.z = 0.4;
marker.color.r = 1.0;
marker.color.b = 0.0;
marker.color.g = 0.0;
marker.color.a = 1.0;
for (auto &p : obstacles_container) {
auto index = p.second.upperBoundByTime(current_time);
if (index > 0) {
index -= 1;
}
auto &obj = p.second.at(index);
auto t = ObstacleContainer::getTimeFromTrackedObject(&obj);
auto abs_t = std::abs(t - current_time);
if (abs_t < 0.01) { // 当前帧存在
// 可视化
marker.id = marker_id++;
ROS_MSGS(geometry_msgs, Point) point;
drawBBox(marker, obj.obj().size());
double heading = obj.obj().angle();
ROS_MSGS(geometry_msgs, Quaternion) quaternion;
quaternion.z = sin(heading / 2);
quaternion.w = cos(heading / 2);
marker.pose.orientation = quaternion;
marker.pose.position.x = obj.longitude_p(); // UTM
marker.pose.position.y = obj.latitude_p();
global_to_local.transform(marker.pose.position.x, marker.pose.position.y);
marker_array.markers.emplace_back(marker);
marker.points.clear();
marker.id = marker_id++;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.pose.position.x = 0;
marker.pose.position.y = 0;
int path_point_size = 50;
for (int k = 0; k < path_point_size; k++) {
auto &obj = p.second.at(index);
point.x = obj.longitude_p(); // UTM
point.y = obj.latitude_p();
global_to_local.transform(point.x, point.y);
marker.points.push_back(point);
if (index == 0) break;
--index;
}
marker_array.markers.emplace_back(marker);
marker.points.clear();
}
}
int id_temp = marker_id;
while (marker_id < last_marker_size) {
marker.id = marker_id++;
marker.action = marker.DELETE;
marker_array.markers.emplace_back(marker);
}
if (id_temp < last_marker_size)
last_marker_size = id_temp;
else
last_marker_size = marker_array.markers.size();
ROS_PUBLISH(obj_trajectory, marker_array);
}
6.障碍物数据可视化效果
如下图所是，可视化了历史轨迹和box。
下图为算法耗时，0.3ms左右。
总结
本文介绍了rosbag相关知识，并基于rosbag数据进行障碍物数据解析和可视化。实际上分别写了模块组件类、容器类、可视化类等，算法架构设计花了不少时间，可视化不太熟悉，浪费了点时间，后面可能不能天天更新了，时间有限。
标签：obj,msgs,Rosbag,bag,实例,time,marker,解析,rosbag From： https://blog.csdn.net/c15901088098/article/details/141199085