how to save an sensor_msgs/Image to a file?

asked 2011-11-21 16:01:45 -0500

spagi
23 ● 2 ● 2 ● 7

Hi all. I am trying to pass an image from a Ros node(C++) to a ROSjava node. I managed to make the rosjava node to subscribe to the others topic. The C++ is publishing an image topic and the Rosjava i supposed to write the image in a file.

The problem is that I cannot find an equivalent on Rosjava for the CVBridge that is used in C++ to make the image message a Iplimage file and save it. What I currently do is try to put the message in a byte[] and then read it in a file. But the result is an jpg file 4 times bigger that the one sent and unreadable. Here is the code I use for the C++ node:

int main(int argc, char **argv)

{    
  ros::init(argc, argv, "imagesender");    
  ros::NodeHandle n;    
  ros::Publisher image_pub = n.advertise<sensor_msgs::Image>("image/mysender", 1);

  printf("Loading image...\n");
  IplImage* imgsrc = cvLoadImage( "/home/spagi/ros_workspace/jpgtransfer/src/testimage.jpg" );
  if(!imgsrc){ printf("Could not load image file\n");}
  else{printf("Image Loaded Sucessfully\n");}

  sensor_msgs::ImagePtr imgmsg = sensor_msgs::CvBridge::cvToImgMsg(imgsrc, "bgr8");

  ros::Rate loop_rate(1);
  while (ros::ok())
  {  
    image_pub.publish(imgmsg);  
    ros::spinOnce();
    loop_rate.sleep();
  }    
  return 0;
}

And here the code for the Rosjava node:

 @Override
  public void main(Node node) {

      final FileInputStream fileInputStream=null;

    Preconditions.checkState(this.node == null);
    this.node = node;
    try {
      //final Log log = node.getLog();
      System.out.println("Started main node");
      node.newSubscriber("image/mysender", "sensor_msgs/Image",
          new MessageListener<org.ros.message.sensor_msgs.Image>() {
            @Override
            public void onNewMessage(org.ros.message.sensor_msgs.Image message) {
                System.out.println("Got a new Image message");
                if(x==0){
                    x++;                    
                byte[] byteArray = message.data;
                try {

                //convert array of bytes into file
                FileOutputStream fileOuputStream = 
                          new FileOutputStream("/home/spagi/Desktop/javaccimage.jpg"); 
                fileOuputStream.write(byteArray);
                fileOuputStream.close();            
                System.out.println("Done");                 
                }catch(Exception e){
                    e.printStackTrace();
                }
                }                   
            }

          });
    } catch (Exception e) {
      if (node != null) {
        node.getLog().fatal(e);
      } else {
        e.printStackTrace();
      }
    }
  }

So again my problem is what to do with that message object and how can I make it a file. I dont know if there is an alternative for the publisher on the :

sensor_msgs::CvBridge::cvToImgMsg(imgsrc, "bgr8");