项目中有个环视功能(最简单的那种,需要将4个摄像头捕捉的原始画面拼到一张图上),需要4个Subscribers和1个Publisher。主要参考这一篇官方文档
http://wiki.ros.org/message_filters
和这几篇文章
https://blog.csdn.net/chishuideyu/article/details/77479758
https://blog.csdn.net/orange_littlegirl/article/details/97425696
https://blog.csdn.net/heyijia0327/article/details/45567373
https://blog.csdn.net/Kevin_Xie86/article/details/112799273
官网中的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;
}
几点说明:
- 每张原始图片大小是1280x720,resize成640x320之后,放在一张图片panoramic中;
- 有的头文件是多余的,没有去掉;
- Publisher是一个全局变量,有点ugly。
- 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
)
- 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>
再推荐几篇相关文章
https://blog.csdn.net/tiancailx/article/details/103804070
https://blog.csdn.net/u013834525/article/details/80222686