基于ROS话题发布、接收图像

创建相关软件包

在catkin_ws/src目录下新建软件包并编译：

catkin_create_pkg my_image_transport image_transport cv_bridge
cd …
catkin_make
source devel/setup.bash

创建图像发布者程序

新建my_image_transport/src/my_publisher.cpp：

#include 
#include 
#include 
#include 

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_publisher");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("camera/image", 1);
  cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
  cv::waitKey(30);//不断刷新图像，频率时间为delay，单位为ms
  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();

  ros::Rate loop_rate(5);
  while (nh.ok()) {
    pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
}

创建图像订阅者程序

新建my_image_transport/src/my_subscriber.cpp：

#include 
#include 
#include 
#include 

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  try
  {
    cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
    cv::waitKey(30);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "image_listener");
  ros::NodeHandle nh;
  cv::namedWindow("view");
  cv::startWindowThread();
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
  ros::spin();
  cv::destroyWindow("view");
}

创建编译文件 CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(my_image_transport)
add_compile_options(-std=c++11)

find_package(
catkin REQUIRED COMPONENTS
cv_bridge
image_transport
OpenCV REQUIRED
)

set(
LIBS 
${OpenCV_LIBS} 
${catkin_LIBRARIES})
	
catkin_package()

include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)

add_executable(my_publisher my_publisher.cpp)
target_link_libraries(my_publisher ${LIBS})

add_executable(my_subscriber my_subscriber.cpp)
target_link_libraries(my_subscriber ${LIBS})

编写包文件 package.xml

<build_depend>opencv2</build_depend>
<exec_depend>opencv2</exec_depend>

编译软件包

cd ~/catkin_ws
catkin_make 

运行节点

ros节点管理器:

roscore

启动发布者节点：
工作空间目录下：source devel/setup.bash

rosrun my_image_transport my_publisher /home/q/图片/三叶草.jpeg

运行订阅者节点：
工作空间目录下：source devel/setup.bash

rosrun my_image_transport my_subscriber

运行结果：

查看当前活动节点、话题及交互情况

ros::init(argc, argv, "image_publisher");
ros::init(argc, argv, "image_listener");
image_transport::Publisher pub = it.advertise("camera/image", 1);
image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);

查看当前活动节点：rosnode list

/image_listener
/image_publisher
...

查看当前话题：rostopic list

/camera/image
...

查看各节点交互情况：rosrun rqt_graph rqt_graph

ORB_SLAM2 ROS模块结点的编译

export ORB_SLAM2_ROOT_DIR=/home/q/packages/ORB_SLAM2
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/q/packages/ORB_SLAM2/Examples/ROS/ORB_SLAM2
source /home/q/packages/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.sh

ROS Mono

首先在my_image_transport目录下创建图像发布者程序mono_tum.cpp：

#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

void LoadImages(const string &strFile, vector<string> &vstrImageFilenames,
                vector<double> &vTimestamps);

int main(int argc, char** argv)
{
  ros::init(argc, argv, "mono_tum");
  if(argc != 2)
  {
      cerr << endl << "Usage: rosrun my_image_transport mono_tum path_to_sequence" << endl;
      return 1;
  }

  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("/camera/image_raw", 1);

  vector<string> vstrImageFilenames;
  vector<double> vTimestamps;
  string strFile = string(argv[1])+"/rgb.txt";
  LoadImages(strFile, vstrImageFilenames, vTimestamps);

  int nImages = vstrImageFilenames.size();

  cv::Mat im;
  // double tframe;
  sensor_msgs::ImagePtr msg;
  std_msgs::Header header;
  ros::Rate loop_rate(5);
  for(int ni = 0; ni < nImages; ni++)
  {
    im = cv::imread(string(argv[1])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
    header.stamp = ros::Time(vTimestamps[ni]);

    if(im.empty())
    {
        cerr << endl << "Failed to load image at: "
             << string(argv[1]) << "/" << vstrImageFilenames[ni] << endl;
        return 1;
    }

    cv::waitKey(30);
    msg = cv_bridge::CvImage(header, "bgr8", im).toImageMsg();

    pub.publish(msg);
    cv::waitKey(1);

    ros::spinOnce();
    loop_rate.sleep();
  }
  
  return 0;
}

void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    ifstream f;
    f.open(strFile.c_str());

    // skip first three lines
    string s0;
    getline(f,s0);
    getline(f,s0);
    getline(f,s0);

    while(!f.eof())
    {
        string s;
        getline(f,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            string sRGB;
            ss >> t;
            vTimestamps.push_back(t);
            ss >> sRGB;
            vstrImageFilenames.push_back(sRGB);
        }
    }
}

mono CMakeLists.txt文件

add_executable(mono_tum src/mono_tum.cpp)
target_link_libraries(mono_tum ${LIBS})

启动 Mono发布者节点

roscore
rosrun my_image_transport mono_tum /home/q/ros_projects/image_transport_catkin_ws/src/my_image_transport/dateset/rgbd_dataset_freiburg1_xyz
这个程序就是用ros把数据集里面的图片以话题`/camera/image_raw`
的形式发布出去，我们打开rviz，订阅这个话题可以看看到图片。

启动 ORB_SLAM2 Mono

rosrun ORB_SLAM2 Mono /home/q/packages/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/q/packages/ORB_SLAM2/Examples/Monocular/TUM1.yaml
这个程序的是订阅

ROS Stereo

首先在my_image_transport目录下创建图像发布者程序
左相机节点stereo_left_kitti.cpp：

http://ttshun.com/2018/08/12/ORB_SLAM2%E5%AD%A6%E4%B9%A0%E4%B9%8B%E8%BF%90%E8%A1%8CROS%E6%A8%A1%E5%9D%97/
http://ttshun.com/2018/05/23/C++%E4%B8%AD%E4%BD%BF%E7%94%A8YAML%E8%AF%AD%E8%A8%80/
http://ttshun.com/2018/08/11/ROS%E5%AD%A6%E4%B9%A0%E4%B9%8BOpenCV%E5%9B%BE%E5%83%8F%E3%80%81ROS%20Image%E8%BD%AC%E6%8D%A2%E6%8E%A5%E5%8F%A3cv_bridge/

更改 ORBSLAM2 下面的 CMakeLists.txt文件

Node for stereo camera ros_stereo_eric

rosbuild_add_executable(Stereo_eric
src/ros_stereo_eric.cc
)

target_link_libraries(Stereo_eric
${LIBS}
)
运行截图：