新建ros工程并运行

原创已于 2024-11-07 14:03:41 修改 · 396 阅读

CC 4.0 BY-SA版权

文章标签：

于 2024-09-02 10:21:23 首次发布

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/Imu.h>
#include <livox_ros_driver/CustomMsg.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <boost/filesystem.hpp>
#include <fstream>
#include <iomanip> // 用于设置浮点数精度

namespace fs = boost::filesystem;

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

    std::string bag_file = "/home/chenqi/catkin_ws/Data/hku1.bag";
    std::string output_dir = "/home/chenqi/catkin_ws/Data/output_hkui_data";
    
    // 创建输出目录
    if (!fs::exists(output_dir)) {
        fs::create_directories(output_dir);
        ROS_INFO("Created output directory: %s", output_dir.c_str());
    }

    // 创建子目录
    std::string left_camera_dir = output_dir + "/left_camera";
    std::string right_camera_dir = output_dir + "/right_camera";
    std::string lidar_dir = output_dir + "/lidar";

    if (!fs::exists(left_camera_dir)) {
        fs::create_directories(left_camera_dir);
        ROS_INFO("Created left camera directory: %s", left_camera_dir.c_str());
    }
    if (!fs::exists(right_camera_dir)) {
        fs::create_directories(right_camera_dir);
        ROS_INFO("Created right camera directory: %s", right_camera_dir.c_str());
    }
    if (!fs::exists(lidar_dir)) {
        fs::create_directories(lidar_dir);
        ROS_INFO("Created lidar directory: %s", lidar_dir.c_str());
    }

    // 打开rosbag文件
    rosbag::Bag bag;
    try {
        bag.open(bag_file, rosbag::bagmode::Read);
        ROS_INFO("Opened rosbag file: %s", bag_file.c_str());
    } catch (const std::exception& e) {
        ROS_ERROR("Failed to open rosbag file: %s. Error: %s", bag_file.c_str(), e.what());
        return 1;
    }

    // 创建一个视图来读取所有消息
    rosbag::View view(bag);

    cv_bridge::CvImagePtr cv_ptr;
    
    for (const rosbag::MessageInstance& m : view) {
        std::string topic = m.getTopic();
        ROS_INFO("Processing topic: %s at time %f", topic.c_str(), m.getTime().toSec());

        if (topic == "/left_camera/image/compressed" || topic == "/right_camera/image/compressed") {
            // 处理压缩图像数据
            sensor_msgs::CompressedImageConstPtr img_msg = m.instantiate<sensor_msgs::CompressedImage>();
            if (img_msg != nullptr) {
                ROS_INFO("Received image message from topic: %s", topic.c_str());
                try {
                    cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8);
                } catch (cv_bridge::Exception& e) {
                    ROS_ERROR("cv_bridge exception: %s", e.what());
                    continue;
                }

                // 根据主题选择合适的目录
                std::string image_dir = (topic == "/left_camera/image/compressed") ? left_camera_dir : right_camera_dir;

                // 生成图像文件名
                std::ostringstream img_name_stream;
                img_name_stream << image_dir << "/image_" << std::fixed << std::setprecision(6) << m.getTime().toSec() << ".jpg";
                std::string img_name = img_name_stream.str();

                // 保存图像
                if (cv::imwrite(img_name, cv_ptr->image)) {
                    ROS_INFO("Saved image: %s", img_name.c_str());
                } else {
                    ROS_ERROR("Failed to save image: %s", img_name.c_str());
                }
            } else {
                ROS_WARN("Failed to instantiate image message from topic: %s", topic.c_str());
            }
        } else if (topic == "/livox/imu") {
            // 处理IMU数据
            sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
            if (imu_msg != nullptr) {
                std::ofstream imu_file(output_dir + "/imu_data.txt", std::ios_base::app);
                imu_file << "timestamp: " << std::fixed << std::setprecision(6) << m.getTime().toSec() << "\n";
                imu_file << "orientation: [" << imu_msg->orientation.x << ", "
                         << imu_msg->orientation.y << ", "
                         << imu_msg->orientation.z << ", "
                         << imu_msg->orientation.w << "]\n";
                imu_file << "angular_velocity: [" << imu_msg->angular_velocity.x << ", "
                         << imu_msg->angular_velocity.y << ", "
                         << imu_msg->angular_velocity.z << "]\n";
                imu_file << "linear_acceleration: [" << imu_msg->linear_acceleration.x << ", "
                         << imu_msg->linear_acceleration.y << ", "
                         << imu_msg->linear_acceleration.z << "]\n\n";
                ROS_INFO("Saved IMU data at time: %f", m.getTime().toSec());
            } else {
                ROS_WARN("Failed to instantiate IMU message from topic: %s", topic.c_str());
            }
        } else if (topic == "/livox/lidar") {
            // 处理LiDAR数据
            livox_ros_driver::CustomMsgConstPtr lidar_msg = m.instantiate<livox_ros_driver::CustomMsg>();
            if (lidar_msg != nullptr) {
                std::ostringstream lidar_file_stream;
                lidar_file_stream << lidar_dir << "/lidar_data_" << std::fixed << std::setprecision(6) << m.getTime().toSec() << ".txt";
                std::string lidar_file = lidar_file_stream.str();
                std::ofstream ofs(lidar_file);
                
                // 遍历所有点并写入文件
                for (const auto& point : lidar_msg->points) {
                    ofs << point.x << " " << point.y << " " << point.z << "\n";
                }
                ROS_INFO("Saved LiDAR data at time: %f", m.getTime().toSec());
            } else {
                ROS_WARN("Failed to instantiate LiDAR message from topic: %s", topic.c_str());
            }
        }
    }

    bag.close();
    ROS_INFO("Data extraction completed. Files are saved in %s.", output_dir.c_str());

    return 0;
}

### 设置独立的 ROS 工程

1. **创建 ROS 工程**:
   ```bash
   cd ~/catkin_ws/src
   catkin_create_pkg bag_extractor roscpp sensor_msgs cv_bridge livox_ros_driver rosbag
   ```

2. **将代码放入 src 文件夹**:
将上面的 C++ 代码保存为 `bag_extractor_node.cpp`，放入 `bag_extractor/src/` 文件夹中。

3. **编辑 `CMakeLists.txt`**:
在 `CMakeLists.txt` 中添加以下内容以编译节点：

cmake_minimum_required(VERSION 3.0.2)
project(bag_extractor)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  cv_bridge
  livox_ros_driver
  rosbag
)

find_package(Boost REQUIRED COMPONENTS filesystem system)

find_package(OpenCV REQUIRED)

catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(bag_extractor_node src/bag_extractor_node.cpp)

target_link_libraries(bag_extractor_node
  ${catkin_LIBRARIES}
  ${Boost_LIBRARIES}
  ${OpenCV_LIBRARIES}
)

4、编译

cd ~/catkin_ws
catkin_make

source devel/setup.bash

5、验证包是否识别

使用 rospack 或 roscd 命令检查 ROS 是否识别到 bag_extractor 包。

rospack find bag_extractor

6、运行

终端1：

roscore

终端2：

rosrun bag_extractor bag_extractor_node

如果终端2失败，记得检查

7. 检查 ROS 环境变量

有时，ROS 环境变量可能未正确设置。通过以下命令查看 ROS_PACKAGE_PATH 是否包含你的工作空间：
echo $ROS_PACKAGE_PATH

你应该能看到类似 ~/catkin_ws/src 的路径。如果没有，确保已经正确源化了 setup.bash。

需要：source devel/setup.bash

### 说明

- **话题订阅**: C++ 代码中通过 `rosbag::View` 和 `MessageInstance` 实例化消息，处理与 `.bag` 文件中的各个话题相关的不同消息类型。
- **图像处理**: 使用 `cv_bridge` 和 OpenCV 将图像从 ROS 格式转换为 OpenCV 格式并保存。
- **IMU 和 LiDAR 数据处理**: 分别读取 IMU 和 LiDAR 数据，并保存到文件中。

通过这个 C++ 实现，您可以独立处理 ROS bag 文件，并将数据提取到指定目录下。