140 lines
6.2 KiB
C++
140 lines
6.2 KiB
C++
//
|
|
// Created by Podmogilnyi Ivan on 11.08.2022.
|
|
//
|
|
#include <ros/ros.h>
|
|
#include <sensor_msgs/Image.h>
|
|
#include <message_filters/subscriber.h>
|
|
#include <message_filters/sync_policies/approximate_time.h>
|
|
#include <message_filters/time_synchronizer.h>
|
|
|
|
#include <cv_bridge/cv_bridge.h>
|
|
#include <opencv2/core.hpp>
|
|
#include <opencv2/imgproc/imgproc.hpp>
|
|
#include <opencv2/imgcodecs.hpp>
|
|
#include <opencv2/highgui.hpp>
|
|
|
|
std::vector<sensor_msgs::Image> sensor_images;
|
|
ros::Publisher pub_res, pub_mask, pub_recv_curr_image;
|
|
|
|
// sMsg - sensor message. rMsg - render message
|
|
void rendered_image_callback(sensor_msgs::Image::ConstPtr sMsg, sensor_msgs::Image::ConstPtr rMsg){
|
|
// publish the left image to the topic needed for
|
|
// pub_recv_curr_image.publish(sMsg);
|
|
|
|
cv::Mat image, mask;
|
|
cv_bridge::CvImagePtr bridge_img = cv_bridge::toCvCopy(rMsg, "bgr8");
|
|
image = bridge_img->image;
|
|
ROS_INFO_STREAM("The image encoding is: " << bridge_img->encoding);
|
|
mask = image.clone();
|
|
cv::cvtColor(mask, mask, cv::COLOR_BGR2GRAY);
|
|
|
|
// find the appropriate image from msgs
|
|
cv::Mat current_sensor_image;
|
|
cv_bridge::CvImagePtr bridge_sensor_img = cv_bridge::toCvCopy(sMsg, "bgr8");
|
|
current_sensor_image = bridge_sensor_img->image;
|
|
|
|
ROS_INFO_STREAM("The message sequence number: " << rMsg->header.seq);
|
|
ROS_INFO_STREAM("The sensor_message sequence number: " << sMsg->header.seq);
|
|
|
|
ROS_INFO_STREAM("The message stamp is: " << std::fixed << std::setprecision(9) << rMsg->header.stamp.toSec());
|
|
ROS_INFO_STREAM("The sensor_message stamp is: " << std::fixed << std::setprecision(9) << sMsg->header.stamp.toSec());
|
|
|
|
bool found_flag = true;
|
|
// since there's the exact match in the timestamps, we don't need to find the closest
|
|
// timestamp, therefore, no need to use the Synchronized algorithm from message_filters in ROS.
|
|
// for (int i=sensor_images.size()-1;i>=0;i--){
|
|
// if (sensor_images[i].header.stamp == msg.header.stamp){
|
|
// cv_bridge::CvImagePtr bridge_sensor_img = cv_bridge::toCvCopy(sensor_images[i], "bgr8");
|
|
// current_sensor_image = bridge_sensor_img->image;
|
|
// found_flag = true;
|
|
// break;
|
|
// }
|
|
// }
|
|
|
|
if (found_flag){
|
|
ROS_INFO_STREAM("imrender: 1");
|
|
ROS_INFO_STREAM("imrender: 1.2");
|
|
cv::Mat masked(image.rows, image.cols, CV_8UC3, cv::Scalar(0, 0, 0));
|
|
masked = current_sensor_image.clone();
|
|
ROS_INFO_STREAM("imrender: 2" << image.rows << " " << image.cols << " " << current_sensor_image.rows <<
|
|
" " << current_sensor_image.cols << " " << masked.rows << " " << masked.cols);
|
|
//copy the non zero pixels to masked from image.
|
|
for (int i=0;i<mask.rows;i++){
|
|
for (int j=0;j<mask.cols;j++){
|
|
if (mask.at<cv::uint8_t>(i, j) != 0){
|
|
masked.at<cv::Vec3b>(i, j) = image.at<cv::Vec3b>(i, j);
|
|
}
|
|
}
|
|
}
|
|
|
|
ROS_INFO_STREAM("current image type: " << current_sensor_image.type());
|
|
ROS_INFO_STREAM("rendered image type: " << image.type());
|
|
ROS_INFO_STREAM("masked image type: " << masked.type());
|
|
// cv::bitwise_and(current_sensor_image, image, masked, mask);
|
|
ROS_INFO_STREAM("imrender: 3");
|
|
|
|
std::string fn = std::to_string(rMsg->header.stamp.toSec()) + "_cam0.png";
|
|
cv::imwrite(fn, masked);
|
|
|
|
std_msgs::Header header;
|
|
header.stamp = rMsg->header.stamp;
|
|
header.frame_id = "cam0";
|
|
sensor_msgs::ImagePtr masked_msg = cv_bridge::CvImage(header, "bgr8", masked).toImageMsg();
|
|
sensor_msgs::ImagePtr mask_msg = cv_bridge::CvImage(header, "mono8", mask).toImageMsg();
|
|
ROS_INFO_STREAM("imrender: 4");
|
|
pub_res.publish(*masked_msg);
|
|
pub_mask.publish(*mask_msg);
|
|
ROS_INFO_STREAM("imrender: 5");
|
|
// cv::imwrite(fn, masked);
|
|
}
|
|
else{
|
|
ROS_INFO("Not found an image with the appropriate timestamp.");
|
|
}
|
|
}
|
|
|
|
void sensor_image_callback(sensor_msgs::Image msg){
|
|
sensor_images.push_back(msg);
|
|
ROS_INFO_STREAM("The sensor_images size is: " << sensor_images.size());
|
|
ROS_INFO_STREAM("The sensor_image seq number is: " << msg.header.seq);
|
|
|
|
// cv::Mat image;
|
|
// cv_bridge::CvImagePtr bridge_msg = cv_bridge::toCvCopy(msg, "bgr8");
|
|
// ROS_INFO_STREAM("imrender: 6" << std::endl << "received image encoding is: " << msg.encoding);
|
|
// current_sensor_image = bridge_msg->image;
|
|
// ROS_INFO_STREAM("imrender: 7" << std::endl << "the number of channels in opencv image: " << current_sensor_image.channels());
|
|
// if (current_sensor_image.channels() == 1){
|
|
// cv::cvtColor(current_sensor_image, current_sensor_image, cv::COLOR_GRAY2BGR);
|
|
// }
|
|
//// cv::imshow("currr_image", current_sensor_image);
|
|
//// cv::waitKey(1);
|
|
// auto current_sensor_image_msg = cv_bridge::CvImage(msg.header, "bgr8", current_sensor_image).toImageMsg();
|
|
// pub_recv_curr_image.publish(*current_sensor_image_msg);
|
|
// ROS_INFO_STREAM("imrender: 8");
|
|
}
|
|
|
|
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
|
|
|
|
int main(int argc, char* argv[]){
|
|
ros::init(argc, argv, "image_render_combine");
|
|
|
|
ros::NodeHandle nh;
|
|
sensor_images = {};
|
|
|
|
ROS_INFO("Hello world!");
|
|
|
|
ros::Subscriber sub_rend_image, sub_sensor_image;
|
|
|
|
// sub_sensor_image = nh.subscribe("/stereo/left/image_raw", 1000, &sensor_image_callback);
|
|
// sub_rend_image = nh.subscribe("/ov_msckf/rendered", 1000, &rendered_image_callback);
|
|
auto sensor_image_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, "/stereo/left/image_raw", 1);
|
|
auto rend_image_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, "/ov_msckf/rendered", 1);
|
|
auto sync = std::make_shared<message_filters::Synchronizer<sync_pol>>(sync_pol(10), *sensor_image_sub, *rend_image_sub);
|
|
sync->registerCallback(boost::bind(&rendered_image_callback, _1, _2));
|
|
pub_res = nh.advertise<sensor_msgs::Image>("/ov_msckf/combined", 2);
|
|
pub_mask = nh.advertise<sensor_msgs::Image>("/ov_msckf/mask", 2);
|
|
// pub_recv_curr_image = nh.advertise<sensor_msgs::Image>("/global_fusion_node/sensor_left", 2);
|
|
|
|
|
|
ros::spin();
|
|
}
|