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;
}
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}
)
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>