ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to publish sensor_msgs Image during node creates images

asked 2022-05-12 02:25:40 -0500

Petros ADLATUS gravatar image

updated 2022-05-12 03:26:21 -0500

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;
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-05-14 04:50:34 -0500

Petros ADLATUS gravatar image

updated 2022-05-14 04:51:16 -0500

SOLUTION: I found out, that I don't need the cv::imread() or cv::imdecode(), because the data are already stored in cv::Mat *bgr. So it's just needed to access there with:

image_transport::Publisher pubImage;
cv::bridge::CvImage cv_bird_view(pointCloudMsg->header, "bgr8", *bgr);
pubImage.publish(cv_bird_view.toImageMsg());
pubImage = it.advertise("out_bev_image",1);
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-05-12 02:25:40 -0500

Seen: 353 times

Last updated: May 14 '22