How to publish sensor_msgs Image during node creates images
Hello,
I use a node that subscribes to the PointCloud2
message and converts them to Bird Eye View photos with subsequent storage. While the node is running, the LiDAR scan is displayed in OpenCV windows and the images are saved. My plan is to publish sensor_msgs/Image
at the same time.
I am working with the cv::imread
and cv::imdecode
functions to see if I can also access the raw data, however neither method works so far. I have followed the two sources (http://wiki.ros.org/image_transport / http://wiki.ros.org/image_transport/T...) for the implementation.
Code snippets not complete
Function:
// Global Publishers/Subscribers
ros::Subscriber subPointCloud;
ros::Publisher pubPointCloud;
image_transport::Publisher pubImage;
// create Matrix to store pointcloud data
cv::Mat *heightmap, *hsv, *bgr;
std::vector<int> compression_params;
cv::Mat image;
int fnameCounter = 0;
void DEM(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
{
// Display image
cv::cvtColor(*hsv, *bgr, cv::COLOR_HSV2BGR); // HSV matrix (src) to BGR matrix (dst)
// Plot HSV(colored) and BGR (b/w)
cv::imshow(WIN_NAME, *bgr); // show new HSV matrix
cv::imshow(WIN_NAME2, *heightmap); // show old BGR matrix
// Save image to disk
char filename[100];
// FLAG enable/disable saving function
if (save_to_disk == true)
{
// save JPG format
snprintf(filename, 100, "/home/pkatsoulakos/catkin_ws/images/image_%d.jpg", fnameCounter);
std::cout << filename << std::endl;
// JPG image writing
cv::imwrite(filename, *bgr, compression_params);
image = cv::imread("/home/pkatsoulakos/catkin_ws/tests/filtered_30_30/image_11.png", IMREAD_COLOR);
if (image.empty())
{
std::cout << "!!! Failed imread(): image not found" << std::endl;
}
else
{
std::cout << "succesfully loaded" << std::endl;
}
/*image = cv::imdecode(*bgr, IMREAD_COLOR);
if (image.data == NULL)
{
std::cout <<"!!! Failed imread(): image not found" << std::endl;
}*/
}
++fnameCounter;
// Publish bird_view img
cv_bridge::CvImage cv_bird_view;
cv_bird_view.header.stamp = ros::Time::now();
cv_bird_view.header.frame_id = "out_bev_image";
cv_bird_view.encoding = "bgr8";
cv_bird_view.image = image;
pubImage.publish(cv_bird_view.toImageMsg());
// Output Image
//sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
//pubImage.publish(msg);
}
Main:
int main(int argc, char** argv)
{
ROS_INFO("Starting LIDAR Node");
ros::init(argc, argv, "lidar_node");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
// Setup Image Output Parameters
fnameCounter = 0;
lowest = FLT_MAX;
/* PNG compression param
compression_params.push_back(IMWRITE_PNG_COMPRESSION);
A higher value means a smaller size and longer compression time. Default value is 3.
compression_params.push_back(9); */
// JPG compression param
compression_params.push_back(IMWRITE_JPEG_QUALITY);
// from 0 to 100 (the higher is the better). Default value is 95.
compression_params.push_back(95);
// subscriber and publisher
subPointCloud = nh.subscribe<sensor_msgs::PointCloud2>("/pointcloud", 2, DEM);
pubPointCloud = nh.advertise<sensor_msgs::PointCloud2> ("/heightmap/pointcloud", 1);
pubImage = it.advertise("out_bev_image",1);
ros::spin();
return 0;
}