ros图片转bag包

最新推荐文章于 2023-06-23 16:09:50 发布

David_By

最新推荐文章于 2023-06-23 16:09:50 发布

阅读量1.2k

点赞数 1

分类专栏： Linux 文章标签： opencv 计算机视觉人工智能

本文链接：https://blog.csdn.net/JanKin_BY/article/details/126227062

版权

Linux 专栏收录该内容

52 篇文章 5 订阅

订阅专栏

图片标定需将图片转换为bag包

CAMKELISTS

cmake_minimum_required(VERSION 3.0.2)
project(imgtobag)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
# 寻找OpenCV库
find_package( OpenCV 3 REQUIRED )
# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} )

find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  rosbag
  roscpp
  rospy
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES imgtobag
#  CATKIN_DEPENDS cv_bridge rosbag roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
  ${rosbag_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(node src/torosbag.cpp)
target_link_libraries(node
  ${catkin_LIBRARIES}
  ${PCL_LIBRARIES}
  ${rosbag_LIBRARIES}
  ${OpenCV_LIBS}
)

torosbag.cpp

#include <string>
#include <ros/console.h>
#include <rosbag/bag.h>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <vector>
#include <sys/types.h>
#include <dirent.h>
#include <unistd.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;

void GetFileNames(string path,vector<string>& filenames, string con);
void GetFileNamesByGlob(cv::String path,vector<cv::String>& filenames, string con);
int main(int argc, char **argv)
{
    //输入文件和输出文件路径
    string base_dir = "/media/jankin/5622A53322A518CF/linux/picture";
    string img_dir = base_dir + "15_new/";
    string output_bag=base_dir +"res.bag";
    string img_format = ".jpg";//格式
    vector<string> img_names;
    GetFileNames(img_dir, img_names,".jpg");
    cout<<"图片读取完成"<<endl;

    cout<<"----"<<endl;

    ros::Time::init();
    rosbag::Bag bag;
    bag.open(output_bag, rosbag::bagmode::Write);
    int seq = 0;
    vector<string>::iterator it;
    for(it = img_names.begin(); it != img_names.end();it++)//todo 之后改成图片数量的多少
    {
        string tmp = *it;
        //cout<<tmp<<endl;
        string strImgFile =  img_dir + tmp + img_format;
        usleep(200000);//4hz
        ros::Time timestamp_ros = ros::Time::now();
        
        // --- for image ---//
        cv::Mat img = cv::imread(strImgFile);
        if (img.empty())
            cout<<"图片为空: "<<strImgFile<<endl;
        cv_bridge::CvImage ros_image;
        sensor_msgs::ImagePtr ros_image_msg;

        ros_image.image = img;
        ros_image.encoding = "bgr8";
        //cout<<"debug_______"<<endl;
        //ros::Time timestamp_ros2 = ros::Time::now();
        ros_image_msg = ros_image.toImageMsg();
        ros_image_msg->header.seq = seq;
        ros_image_msg->header.stamp = timestamp_ros;
        ros_image_msg->header.frame_id = "/image_raw";

        bag.write("/cam", ros_image_msg->header.stamp, ros_image_msg);
        cout<<"write frame: "<<seq<<endl;
        seq++;
    }

    cout<<"---end---"<<endl;

    return 0;
}

//con:文件格式 form:文件命名形式
void GetFileNames(string path,vector<string>& filenames, string con)
{
    DIR *pDir;
    struct dirent* ptr;
    string filename, format, name, name2;

    if(!(pDir = opendir(path.c_str())))
        return;
    int num=0;
    while((ptr = readdir(pDir))!=0) 
    {
        //跳过.和..文件
        if(strcmp(ptr->d_name, ".") == 0 || strcmp(ptr->d_name, "..") == 0)
            continue;
        filename = ptr->d_name;
        format = filename.substr(filename.find("."), filename.length());
        //name = filename.substr(0, filename.find("."));
        name = filename.substr(0, filename.find("."));
        cout<<filename<<"\t"<<name<<"\t"<<format<<endl;

        if(format == con)//也可以添加对文件名的要求
        {
            filenames.push_back(name);
            
            num++;
        }        
    }
    std::cout<<"file size of:"<<filenames.size()<<"****"<<num<<std::endl;
    closedir(pDir);
}

会生成一个res.bag，帧率可能不完全是5hz需自己优化