好久没有更新博客！早晨听了关于一个竞赛的线上培训，今日份心血来潮啦！

  例：环视功能，比如需要将4个摄像头捕捉的原始画面拼到一张图上，需要四个Subscribers和一个Publisher；主要参考的是官方的一个文档，网站我列在这里了。

http://wiki.ros.org/message-filters

然后官网的demo只有一个，就是这几行：

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
}

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

  ros::NodeHandle nh;

  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

然后进入正题，就是在一个节点中，同时订阅4个topic，将得到4个message放进一个callback里面做处理，处理完以后再发出来，最后可以用rviz可视化。换句话说，就是同步多个传感器，或者说是多传感器融合。上代码：

#include <iostream>

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include "sensor_msgs/image_encodings.h"
#include <sensor_msgs/Image.h>
#include <boost/bind.hpp>

using namespace std;
using namespace cv;
using namespace sensor_msgs;
using namespace message_filters;


image_transport::Publisher pub;

void callback(const sensor_msgs::ImageConstPtr& image_source1, const sensor_msgs::ImageConstPtr& image_source2, const sensor_msgs::ImageConstPtr& image_source3, const sensor_msgs::ImageConstPtr& image_source4)
{
  // Solve all of perception here...
  cout << "Test is ok." << endl;
	//panoramic is the final picture
    cv::Mat panoramic(720, 1280, CV_8UC3, cv::Scalar(0, 0, 0));
    
    //front
    cv_bridge::CvImagePtr cv_image1 = cv_bridge::toCvCopy(image_source1, sensor_msgs::image_encodings::BGR8);
    cv::Mat front = cv_image1->image.clone();
    cv::Mat front_640x360;
    cv::resize(front, front_640x360, cv::Size(640, 360));
    //right
    cv_bridge::CvImagePtr cv_image2 = cv_bridge::toCvCopy(image_source2, sensor_msgs::image_encodings::BGR8);
    cv::Mat right_640x360;
    cv::Mat right = cv_image2->image.clone();
    cv::resize(right, right_640x360, cv::Size(640, 360));
    //rear
    cv_bridge::CvImagePtr cv_image3 = cv_bridge::toCvCopy(image_source3, sensor_msgs::image_encodings::BGR8);
    cv::Mat rear_640x360;
    cv::Mat rear = cv_image3->image.clone();
    cv::resize(rear, rear_640x360, cv::Size(640, 360));
    //left
    cv_bridge::CvImagePtr cv_image4 = cv_bridge::toCvCopy(image_source4, sensor_msgs::image_encodings::BGR8);
    cv::Mat left = cv_image4->image.clone();
    cv::Mat left_640x360;
    cv::resize(left, left_640x360, cv::Size(640, 360));
    
    //fill front into panoramic
    for (int i = 0; i < 360; i = i + 1) {
        for (int j = 0; j < 640; j = j + 1) {
            panoramic.at<cv::Vec3b>(i, j) = front_640x360.at<cv::Vec3b>(i, j);
        }
    }
    //fill rear into panoramic
    for (int i = 0; i < 360; i = i + 1) {
        for (int j = 640; j < 1280; j = j + 1) {
            panoramic.at<cv::Vec3b>(i, j) = rear_640x360.at<cv::Vec3b>(i, j - 640);
        }
    }
    //fill left into panoramic
    for (int i = 360; i < 720; i = i + 1) {
        for (int j = 0; j < 640; j = j + 1) {
            panoramic.at<cv::Vec3b>(i, j) = left_640x360.at<cv::Vec3b>(i - 360, j);
        }
    }
    //fill right into panoramic
    for (int i = 360; i < 720; i = i + 1) {
        for (int j = 640; j < 1280; j = j + 1) {
            panoramic.at<cv::Vec3b>(i, j) = right_640x360.at<cv::Vec3b>(i - 360, j - 640);
        }
    }

	//publish msg
    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", panoramic).toImageMsg();
    pub.publish(msg);
}


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

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

  message_filters::Subscriber<sensor_msgs::Image> sub_1(nh, "/miivii_gmsl_ros/camera1", 1);
  message_filters::Subscriber<sensor_msgs::Image> sub_2(nh, "/miivii_gmsl_ros/camera2", 1);
  message_filters::Subscriber<sensor_msgs::Image> sub_3(nh, "/miivii_gmsl_ros/camera3", 1);
  message_filters::Subscriber<sensor_msgs::Image> sub_4(nh, "/miivii_gmsl_ros/camera4", 1);
  TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> sync(sub_1, sub_2, sub_3, sub_4, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4));
  //You can register multiple callbacks with the registerCallbacks() method. They will get called in the order they are registered.
  
  ros::spin();

  return 0;
}

！！！在此说明一下：

1.每张图原始大小是1280*720，resize成640*320以后，放一张图片panoramic里；

2.有的头文件是多余的没有去掉；

3.Publisher是一个全局变量；

4.CMakeLists.txt.

cmake_minimum_required(VERSION 2.8)

project(panoramic_ground)

SET(CMAKE_INCLUDE_CURRENT_DIR ON)

find_package(autoware_msgs REQUIRED)
find_package(catkin REQUIRED COMPONENTS
        cv_bridge
        image_transport
        roscpp
        autoware_msgs
        )
find_package(OpenCV REQUIRED)
find_package(OpenGL REQUIRED)

catkin_package(
        #INCLUDE_DIRS include
        #LIBRARIES ${PROJECT_NAME}
        #  LIBRARIES vision_lane_detect
        CATKIN_DEPENDS
          cv_bridge
          image_transport
          roscpp
          autoware_msgs
        #  DEPENDS system_lib
)

include_directories(include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
  ${linux_INCLUDE_DIR}
)

INCLUDE_DIRECTORIES(include /usr/include)

add_executable(${PROJECT_NAME} "main.cpp")

target_link_libraries(${PROJECT_NAME}
        ${catkin_LIBRARIES}
        ${OpenCV_LIBRARIES}
        ${OPENGL_LIBRARIES}
        ${catkin_LIB_DIRS}
)

add_dependencies(${PROJECT_NAME}
    ${catkin_EXPORTED_TARGETS}
    autoware_msgs_generate_messages_cpp
)

5.package.xml

<?xml version="1.0"?>
<package>
  <name>panoramic_ground</name>
  <version>0.0.0</version>
  <description>The panoramic panoramic_ground</description>
  <maintainer email="dl.yjw@163.com">abc</maintainer>
  <license>TODO</license>


  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>opencv2</build_depend>
  <build_depend>autoware_msgs</build_depend>

  <run_depend>roscpp</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>cv_bridge</run_depend>
  <run_depend>image_transport</run_depend>
  <run_depend>opencv2</run_depend>
  <run_depend>message_runtime</run_depend>
  <run_depend>autoware_msgs</run_depend>

  <export>
  </export>
</package>

结束！

今天就到这里啦，其他的改日再议QAQ