用map和view实现rosbag数据逐帧打印

最新推荐文章于 2024-03-10 23:04:04 发布

压力鼠鼠睡不好

最新推荐文章于 2024-03-10 23:04:04 发布

阅读量381

点赞数 7

文章标签： c++

本文链接：https://blog.csdn.net/Chi0217/article/details/135981147

版权

本文介绍了一个C++程序，用于从rosbag文件中提取所有主题及其消息数量，允许用户选择查看并逐帧打印选定topic的消息，以txt格式保存。程序还涉及自定义消息类型和CMakeLists.txt及package.xml文件的配置。

摘要由CSDN通过智能技术生成

代码的主要功能是首先输出rosbag的所有主题和消息数量、信息类型，然后让你来选择查看哪一个topic，然后把这个topic里面的所有类型的消息都逐帧打印出来，用txt格式保存

#include <ros/ros.h>
#include <iostream>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <set>
#include <map>
#include <fstream>
#include <sstream>
#include <ctime>

// 包含你的消息类型的头文件
#include <test_pkg/GpsPosition.h>
#include <test_pkg/ParkingSpace.h>
// #include <test_pkg/ParkingSpaceArray.h>
#include <test_pkg/cicv_moving_object.h>
// #include <test_pkg/cicv_moving_objects.h>

// 这里用手上的一个rosbag进行举例
// rosbag info显示的话题
// /minibus/bestposition             #5002 msgs    : util/GpsPosition              
// /minibus/fusion_polygon_marker    #1017 msgs    : visualization_msgs/MarkerArray
// /minibus/fusion_velocity_marker   #1017 msgs    : visualization_msgs/MarkerArray
// /minibus/parking_spaces           #5492 msgs    : util/ParkingSpaceArray  这个依赖ParkingSpace.msg也不会改      
// /minibus/polygon_objects          #1017 msgs    : util/cicv_moving_objects 这个依赖cicv_moving_object.msg不会改  
// /minibus/vehicle/info
// 有的包含消息的头文件方不上来是因为visualization开头的包好像没有，只有util开头的，但是其中例如util/ParkingSpaceArray.msg
// 这个文件中又包含了util/ParkingSpace.msg的消息类型，要在cmakelist里面加依赖项啥的，但是折腾了好久还是不会改

int main(int argc, char **argv) {
    ros::init(argc, argv, "bag_reader");
    ros::NodeHandle nh;

    rosbag::Bag bag;
    bag.open("/home/ca/catkin_ws/src/bag_test/yuexiangbag_msg/util/boche.bag", rosbag::bagmode::Read);

    // 创建一个不基于特定主题查询的view
    rosbag::View all_view(bag);

    // 使用map来收集主题以及对应的消息类型和数量
    std::map<std::string, std::pair<std::string, int>> topic_info;

    BOOST_FOREACH(rosbag::MessageInstance const m, all_view) {
        std::string const& topic = m.getTopic();
        std::string const& datatype = m.getDataType();

        // 如果该主题尚未在map中，则添加
        if (topic_info.find(topic) == topic_info.end()) {
            topic_info[topic] = std::make_pair(datatype, 0);
        }
        // 增加该主题的消息计数
        topic_info[topic].second++;
    }

    // 打印所有主题及其消息类型和数量
    std::cout << "Topics in the bag file:" << std::endl;
    for (auto const& entry : topic_info) {
        std::cout << entry.first << " " << entry.second.second << " msgs : " << entry.second.first << std::endl;
    }

    // 询问用户想要查询的主题
    std::string selected_topic;
    std::cout << "Enter the topic you want to query \n 输入你想查询的topic(目前只能查/minibus/bestposition): \n ";
    std::getline(std::cin, selected_topic);

    // 创建一个基于用户选择的主题的view
    std::vector<std::string> topics;
    topics.push_back(selected_topic);
    rosbag::View view(bag, rosbag::TopicQuery(topics));

    // 获取当前时间作为文件名的一部分
    std::time_t now = std::time(nullptr);
    char buf[80];
    std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", std::localtime(&now));
    std::string time_str(buf);

    // 将话题名称中的斜杠替换为下划线，原因为：
    // 文件名中，/minibus/bestposition 是从话题名称直接获取的，其中包含斜杠（/），这通常在文件系统中用作目录分隔符。
    // 因此，程序尝试在 bag_content 目录下创建名为 minibus 的子目录，并在该子目录中创建文件，而这个子目录可能并不存在。
    std::string sanitized_topic = selected_topic;
    std::replace(sanitized_topic.begin(), sanitized_topic.end(), '/', '_');

    // 创建文件名并打开文件
    std::string filename = "/home/ca/catkin_ws/src/test_pkg/bag_content/" + sanitized_topic + "_" + time_str + ".txt";
    std::ofstream file;
    file.open(filename);

    if (!file.is_open()) {
        std::cerr << "Unable to open file: " << filename << std::endl;
        return -1;
    }


    BOOST_FOREACH(rosbag::MessageInstance const m, view) {
    // 检查消息类型并相应地处理
    if (m.getTopic() == selected_topic) {
        // 根据消息类型进行转换并打印
        if (m.isType<test_pkg::GpsPosition>()) {
            test_pkg::GpsPosition::ConstPtr i = m.instantiate<test_pkg::GpsPosition>();
            if (i != nullptr) {
                file << "GpsPosition: " << *i << std::endl;
            }
            // 注释掉的是逐帧打印在终端的代码，现在是保存到文件
            // test_pkg::GpsPosition::ConstPtr i = m.instantiate<test_pkg::GpsPosition>();
            // if (i != nullptr) {
            //     std::cout << "GpsPosition: " << *i << std::endl;
            // }
        }
        else if (m.isType<test_pkg::ParkingSpace>()) {
            test_pkg::ParkingSpace::ConstPtr i = m.instantiate<test_pkg::ParkingSpace>();
            if (i != nullptr) {
                file << "ParkingSpace: " << *i << std::endl;
                // std::cout << "ParkingSpace: " << *i << std::endl;
            }
        }
        else if (m.isType<test_pkg::cicv_moving_object>()) {
            test_pkg::cicv_moving_object::ConstPtr i = m.instantiate<test_pkg::cicv_moving_object>();
            if (i != nullptr) {
                file << "cicv_moving_object: " << *i << std::endl;
                // std::cout << "cicv_moving_object: " << *i << std::endl;
            }
        }
        // 添加其他消息类型的处理逻辑
        // 确保您的消息类型名称和命名空间是正确的。
        // 对于每种类型的消息，我们首先检查是否可以将 MessageInstance 转换为该类型的指针，然后再打印出来。
        // 如果您还有其他类型的消息，您需要为它们添加相似的处理逻辑
        // ...
    }
}
    file.close();  // 写入所有数据后关闭文件

    // 检查文件是否成功写入并关闭
    if (file) {
        // 如果文件流处于良好状态，表示文件成功保存
        std::cout << "文件成功保存：" << filename << std::endl;
    } else {
        // 如果文件流不处于良好状态，表示保存文件时出现错误
        std::cerr << "发生错误，请检查文件：" << filename << std::endl;
    }


    bag.close();
    return 0;
}

还要修改cmakelist.txt

find_package要改成

因为有自己定义的消息，所以要用message_generation和message_runtime

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  rosbag
  message_generation
  message_runtime
  geometry_msgs
)

add_message_files要改成

这个就是自定义的几个消息的位置

add_message_files(
  FILES
  util/GpsPosition.msg
  util/ParkingSpace.msg      
  util/cicv_moving_object.msg 
)

generate_message改成

这个好像是在生成自定义消息的时候要依赖哪些包

generate_messages(
  DEPENDENCIES
  std_msgs
  geometry_msgs
)

catkin_message改成

这个跟上面find_pkg是配套的，gpt对这一块这样解释：

catkin_package指令用于声明当前包的特性和依赖关系，包括它对其他包的依赖（CATKIN_DEPENDS），它提供的库（LIBRARIES），包含的目录（INCLUDE_DIRS），以及非catkin的系统依赖（DEPENDS）

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES test_pkg
 CATKIN_DEPENDS roscpp rospy std_msgs message_runtime geometry_msgs message_generation 
#  DEPENDS system_lib
)

最后build部分要加上

add_executable(readbag src/readbag.cpp)
# 可执行文件名是readbag 源文件地址是readbag

add_dependencies(readbag ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# 在编译readbag之前保证先编译列出的目标，有自定义的消息的时候这条是必要的

target_link_libraries(readbag
    ${catkin_LIBRARIES}
)
# 这个就跟前面的链接上了，${catkin_LIBRARIES}变量包含了所有通过find_package找到的、并通过catkin_package声明的CATKIN_DEPENDS依赖包提供的库

还有package.xml部分要加

这个就是头文件标的一些，还有建立项目的时候的roscpp啥的

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>message_runtime</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>message_generation</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>

哦对，我的文件目录结构