diff --git a/ov_msckf/src/imrender_combine.cpp b/ov_msckf/src/imrender_combine.cpp index f553051..ca4422c 100644 --- a/ov_msckf/src/imrender_combine.cpp +++ b/ov_msckf/src/imrender_combine.cpp @@ -3,6 +3,9 @@ // #include #include +#include +#include +#include #include #include @@ -13,9 +16,9 @@ std::vector sensor_images; ros::Publisher pub_res, pub_mask, pub_recv_curr_image; -void rendered_image_callback(sensor_msgs::Image msg){ +void rendered_image_callback(sensor_msgs::Image::ConstPtr sMsg, sensor_msgs::Image::ConstPtr rMsg){ cv::Mat image, mask; - cv_bridge::CvImagePtr bridge_img = cv_bridge::toCvCopy(msg, "bgr8"); + 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(); @@ -23,17 +26,26 @@ void rendered_image_callback(sensor_msgs::Image msg){ // find the appropriate image from msgs cv::Mat current_sensor_image; - ROS_INFO_STREAM("The message sequence number: " << msg.header.seq); - ROS_INFO_STREAM("The sensor_message sequence number: " << sensor_images[sensor_images.size()-1].header.seq); - bool found_flag = false; - for (int i=sensor_images.size()-1;i>0;i--){ - if (i == msg.header.seq){ - cv_bridge::CvImagePtr bridge_sensor_img = cv_bridge::toCvCopy(sensor_images[i], "bgr8"); - current_sensor_image = bridge_sensor_img->image; - found_flag = true; - break; - } - } + 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"); @@ -57,11 +69,11 @@ void rendered_image_callback(sensor_msgs::Image msg){ // cv::bitwise_and(current_sensor_image, image, masked, mask); ROS_INFO_STREAM("imrender: 3"); - std::string fn = std::to_string(msg.header.stamp.toSec()) + "_cam0.png"; + std::string fn = std::to_string(rMsg->header.stamp.toSec()) + "_cam0.png"; cv::imwrite(fn, masked); std_msgs::Header header; - header.stamp = msg.header.stamp; + 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(); @@ -96,6 +108,8 @@ void sensor_image_callback(sensor_msgs::Image msg){ // ROS_INFO_STREAM("imrender: 8"); } +typedef message_filters::sync_policies::ApproximateTime sync_pol; + int main(int argc, char* argv[]){ ros::init(argc, argv, "image_render_combine"); @@ -106,8 +120,12 @@ int main(int argc, char* argv[]){ 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); +// 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>(nh, "/stereo/left/image_raw", 1); + auto rend_image_sub = std::make_shared>(nh, "/ov_msckf/rendered", 1); + auto sync = std::make_shared>(sync_pol(10), *sensor_image_sub, *rend_image_sub); + sync->registerCallback(boost::bind(&rendered_image_callback, _1, _2)); pub_res = nh.advertise("/ov_msckf/combined", 2); pub_mask = nh.advertise("/ov_msckf/mask", 2); pub_recv_curr_image = nh.advertise("/ov_msckf/recv_curr_image", 2); diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index a6ebb8e..09b094d 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -171,30 +171,30 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ float yCubesVisPos = -6.0; float skew = 5.0; cubeVisPoints = { - {71.215, 32.694, yCubesVisPos}, + {71.215, 32.694, yCubesVisPos+3}, // {81.668, 26.227, yCubesVisPos}, - {84.455, 41.731, yCubesVisPos}, + {84.455, 41.731, yCubesVisPos+3}, // {90.726, 32.825, yCubesVisPos}, - {109.541, 60.415, yCubesVisPos}, + {109.541, 60.415, yCubesVisPos+3}, // {149.264, 69.490, yCubesVisPos}, - {150.645, 89.474, yCubesVisPos}, + {150.645, 89.474, yCubesVisPos+2}, // {198.294, 97.113, yCubesVisPos}, {251.391, 154.108, yCubesVisPos}, // {310.030, 190.208, yCubesVisPos}, {330.554, 183.244, yCubesVisPos}, - {746.086, 450.563, yCubesVisPos-1}, + {746.086, 450.563, yCubesVisPos}, // {792.608, 527.140, yCubesVisPos}, - {883.669, 517.444, yCubesVisPos-1}, - {957.811, 652.638, yCubesVisPos-1}, - {1086.045, 669.317, yCubesVisPos-2}, - {1125.310, 728.214, yCubesVisPos-2}, - {731.996, 425.021, yCubesVisPos-1}, + {883.669, 517.444, yCubesVisPos}, + {957.811, 652.638, yCubesVisPos}, + {1086.045, 669.317, yCubesVisPos}, + {1125.310, 728.214, yCubesVisPos}, + {731.996, 425.021, yCubesVisPos}, - {1186.527, 700.189, yCubesVisPos-2}, + {1186.527, 700.189, yCubesVisPos}, // {1205.432, 681.457, yCubesVisPos}, - {1184.375, 735.604, yCubesVisPos-2}, + {1184.375, 735.604, yCubesVisPos}, // {1232.697, 715.647, yCubesVisPos}, - {1244.767, 784.257, yCubesVisPos-2} + {1244.767, 784.257, yCubesVisPos} }; cashedCubeVisPoints = cubeVisPoints; // End of added by PI @@ -715,16 +715,35 @@ void ROS1Visualizer::publish_images() { sensor_msgs::ImagePtr exl_msg = cv_bridge::CvImage(header, "bgr8", img_history).toImageMsg(); exl_msg_global = exl_msg; + // Get the original image in order to publish it to the topic for rendering with CameraPub. + cv::Mat original_img; + double original_img_ts; + _app->get_active_image(original_img_ts, original_img); + // usually the function returns -1. so, let's fill it with the ts from camera_last_timestamp. + if (original_img_ts == -1){ + original_img_ts = camera_last_timestamp[0]; + } + std_msgs::Header orig_img_msg_header; + // as I remember it's the timestamp in kaist is already in seconds. + ROS_INFO_STREAM("org msg: 1"); + ros::Time orig_stamp(original_img_ts); + ROS_INFO_STREAM("org msg: 2"); + orig_img_msg_header.stamp = orig_stamp; + orig_img_msg_header.frame_id = "cam0"; + sensor_msgs::ImagePtr original_img_message = cv_bridge::CvImage(orig_img_msg_header, "bgr8", original_img).toImageMsg(); + ROS_INFO_STREAM("The camera last timestamp for cam0 is: " << std::fixed << std::setprecision(21) << original_img_ts); + ROS_INFO_STREAM("Original image width and height are: " << original_img.cols << " " << original_img.rows); + // Publish it_pub_tracks.publish(exl_msg); // publish the left image with the visualizations - cv::Range rows(0, img_history.rows); - cv::Range cols(0, img_history.cols / 2); - cv::Mat cropped_image = img_history(rows, cols); - sensor_msgs::ImagePtr sensor_left_msg = cv_bridge::CvImage(header, "bgr8", cropped_image).toImageMsg(); - sensor_left_msg->header.seq = sensor_left_id++; - sensor_left_pub.publish(sensor_left_msg); +// cv::Range rows(0, img_history.rows); +// cv::Range cols(0, img_history.cols / 2); +// cv::Mat cropped_image = img_history(rows, cols); +// sensor_msgs::ImagePtr sensor_left_msg = cv_bridge::CvImage(header, "bgr8", cropped_image).toImageMsg(); +// sensor_left_msg->header.seq = sensor_left_id++; + sensor_left_pub.publish(original_img_message); ROS_INFO("Image publish success!"); bool is_fisheye = (std::dynamic_pointer_cast(_app->get_params().camera_intrinsics.at(0)) != nullptr); @@ -747,6 +766,8 @@ void ROS1Visualizer::publish_images() { camInfoGlob = cameraparams; // Added by PI pub_camera_info.publish(camInfoGlob); + camInfoGlob.header.frame_id = "cam0"; + camInfoGlob.header.stamp = orig_img_msg_header.stamp; camInfoGlob.height = exl_msg->height; camInfoGlob.width = exl_msg->width / 2; pub_cam0_info.publish(camInfoGlob);