How to publish sensor_msgs Image during node creates images

edit

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::imdecodefunctions 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;
}