ros发布图像集_ros 发布图像

cpp文件

需要将图像目录转化为自己电脑目录

#include "ros/ros.h"
#include "opencv2/opencv.hpp"
#include "sensor_msgs/Image.h"
#include "cv_bridge/cv_bridge.h"
#include <string>
#include <fstream>
#include <iomanip>

int main(int argc, char** argv){
    ros::init(argc, argv, "pubimg_node");

    ros::NodeHandle nh;

    ros::Publisher pub_rgb_right  = nh.advertise<cv_bridge::CvImage>("/rgb_image2", 100);
    ros::Publisher pub_depth  = nh.advertise<cv_bridge::CvImage>("/depth", 100);

    bool skip_first_line_imu = true;
    bool skip_first_row_gt   = true;


    std::string time_index_address;
    std::string rgb_right_base_address, depth_base_address;
 
    rgb_right_base_address  = "/home/zxn/dev/dataset/livingroom2/livingroom2-color/";
    time_index_address = "/home/zxn/dev/dataset/livingroom2/times.txt";
    depth_base_address = "/home/zxn/dev/dataset/livingroom2/livingroom2-depth-clean/";
    ros::Time before_imu;
    std::ifstream read_time(time_index_address);
    //read all pictures
    cv_bridge::CvImage cv_ptr_rgb_right, cv_ptr_depth;
    cv_ptr_rgb_right.encoding = "8UC3";//8UC3
    cv_ptr_depth.encoding     = "16UC1";
    std::string one_row_time;
    int index = 0;
    while (getline(read_time, one_row_time) && ros::ok())
    {
        double msg_timestamp = (double)atof(one_row_time.c_str());
        //std::cout<<"timestamp of image "<<std::setprecision(12)<<msg_timestamp<<std::endl;
        msg_timestamp = 1000000000 * msg_timestamp;
        uint64_t int_time = uint64_t(msg_timestamp);
        uint64_t sec = int_time / 1000000000;
        uint64_t nsec = int_time % 1000000000;
        cv_ptr_rgb_right.header.stamp.sec = sec;
        cv_ptr_rgb_right.header.stamp.nsec = nsec;
        cv_ptr_depth.header.stamp.sec      = sec;
        cv_ptr_depth.header.stamp.nsec     = nsec;
        std::string s1 = std::to_string(index);
        while(s1.size() <5)
        {
            s1 = "0" + s1;
        }
        cv::Mat rgb_right = cv::imread(rgb_right_base_address + s1 + ".jpg" , CV_LOAD_IMAGE_COLOR);
        cv::Mat depth  = cv::imread(depth_base_address + s1 + ".png", -1);
        cv_ptr_rgb_right.image = rgb_right;
        cv_ptr_depth.image     =  depth.clone();
        index++;
        before_imu = ros::Time::now();
        int count = 0;
        //publish imu based on its cycle. publish image if imu time and ground truth time is bigger than image time

        pub_rgb_right.publish(cv_ptr_rgb_right);
        pub_depth.publish(cv_ptr_depth);
    }
    //ros::spin();
    std::cout<<"finish..............."<<std::endl;
    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

CMakeLists.txt文件

cmake_minimum_required(VERSION 2.8.3)
project(pubimage)

set(WITH_OPENCV_CONTRIB ON)

if( ${CMAKE_VERSION} VERSION_LESS "3.8.2" )
    set(CMAKE_CXX_STANDARD 14)
else()
    set(CMAKE_CXX_STANDARD 17)
endif()

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -march=native")

if(WITH_OPENCV_CONTRIB)
    message("\nGoing to use OpenCV contrib! (WITH_OPENCV_CONTRIB is set to : ${WITH_OPENCV_CONTRIB}.
Set it to OFF is OpenCV was not compiled with the opencv_contrib modules)\n")
    add_definitions(-DOPENCV_CONTRIB)
endif()

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  sensor_msgs
  cv_bridge
  std_msgs
  message_generation
  geometry_msgs
  image_transport
)

# OpenCV
find_package(OpenCV REQUIRED)

if(OpenCV_VERSION_MAJOR LESS 3)
  message( FATAL_ERROR "OpenCV 3 or 4 is required!  Current version : ${OpenCV_VERSION}" )
endif()

if(OpenCV_VERSION VERSION_LESS "3.3.0")
  message("ENABLE Internal ParallelLoopBodyLambdaWrapper Class definition (OpenCV < 3.3.0)")
  add_definitions(-DOPENCV_LAMBDA_MISSING)
endif()

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES depthnet
#  CATKIN_DEPENDS rospy
#  DEPENDS system_lib
)

add_library(${PROJECT_NAME}
  pubimg.cpp)
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

target_link_libraries(${PROJECT_NAME}
    PUBLIC
      ${catkin_LIBRARIES}
      ${OpenCV_LIBS}
)

add_executable(${PROJECT_NAME}_node pubimg.cpp)

target_link_libraries(
    ${PROJECT_NAME}_node
    PRIVATE
      ${PROJECT_NAME}
)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70

package.xml

<?xml version="1.0"?>
<package>
  <name>pubimage</name>
  <version>0.0.0</version>
  <description>The pubimage package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
  <!-- Example:  -->
  <!-- <maintainer email="zxn@example.com">zxn</maintainer> -->
  <maintainer email="zxn@example.com">zxn</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>GPLv3</license>


  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/VaFRIC_warpper</url> -->


  <!-- Author tags are optional, mutiple are allowed, one per tag -->
  <!-- Authors do not have to be maintianers, but could be -->
  <!-- Example: -->
  <!-- <author email="zxn@example.com">zxn</author> -->

  <!-- The *_depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use run_depend for packages you need at runtime: -->
  <!--   <run_depend>message_runtime</run_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>message_generation</build_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>image_transport</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>cv_bridge</run_depend>
  <run_depend>message_generation</run_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65