Re: [ros-users] how to publish JPEG images with compressed_i…

Top Page
Attachments:
Message as email
+ (text/plain)
+ jpeg.cpp (text/x-c++src)
Delete this message
Reply to this message
Author: Rene Ladan
Date:  
To: ros-users
Subject: Re: [ros-users] how to publish JPEG images with compressed_image_transport ?
Hi,

the attached C++ file publishes a JPEG picture from disk on
/jpeg/image/compressed
(verified with rostopic list -v). However, if I run image_view to
display the picture, I only get
a blank window. There seems to be a discrepancy:

%rostopic list -v

Published topics:
* /rosout [roslib/Log] 2 publishers
* /rosout_agg [roslib/Log] 1 publisher
* /jpeg/image/compressed [sensor_msgs/CompressedImage] 1 publisher

Subscribed topics:
* /time [unknown type] 3 subscribers
* /rosout [roslib/Log] 1 subscriber
* /clock [unknown type] 3 subscribers
* /jpeg/image [unknown type] 1 subscriber

Comands invoked:
%roscore (with ROS_IP set to public IP address and ROS_MASTER_URI to
http://localhost:11311)
%rosrun pkg-name jpeg
%rosrun image_view image_view image:=/jpeg/image image_transport:=compressed

So there is probably something wrong with the code, but the unknown
type /jpeg/image in the
subscriber list also looks suspicious?

Regards,
Rene

2010/3/16 Jeremy Leibs <>:
> The axis_camera node in the texas_drivers stack does exactly this.
>
> https://code.ros.org/trac/wg-ros-pkg/browser/stacks/texas_drivers/trunk/axis_camera/axis.py
>
> The relevant snippets (python code, but C++ would be almost identical):
>
> Create the appropriate publishers ("compressed" and "camera_info"
>
>    self.pub = rospy.Publisher("axis_camera/compressed", CompressedImage, self)
>    self.caminfo_pub = rospy.Publisher("axis_camera/camera_info",
> CameraInfo, self)
>
>
>
> Populate the appropriate messages and publish:
>
>      img = fp.read(content_length)
>
>      msg = CompressedImage()
>      msg.header.stamp = rospy.Time.now()
>      msg.format = "jpeg"
>      msg.data = img
>
>      self.axis.pub.publish(msg)
>
>      cimsg = CameraInfo()
>      cimsg.header.stamp = msg.header.stamp
>      cimsg.width = self.width
>      cimsg.height = self.height
>
>      self.axis.caminfo_pub.publish(cimsg)
>
>
> On Tue, Mar 16, 2010 at 7:10 AM, Rene Ladan <> wrote:
>> Hi,
>>
>> I'm writing a node to publish images from the Surveyor stereo camera
>> (see http://www.surveyor.com/stereo/ ).
>> The camera publishes the images in JPEG format.  The logical thing
>> would be to somehow feed them to the
>> compressed_image_format node (after setting up a handmade header?) to
>> publish them.  Is there any example
>> code which does this?
>>
>> Also, if I understand correctly, the subscriber can just subscribe to
>> the normal image_transport topic, but needs
>> to have extra code to decompress compressed images before using them?
>>
>> Thanks,
>> Rene
>> --
>> http://www.rene-ladan.nl/
>>
>> GPG fingerprint = E738 5471 D185 7013 0EE0  4FC8 3C1D 6F83 12E1 84F6
>> (subkeys.pgp.net)
>> _______________________________________________
>> ros-users mailing list
>>
>> https://code.ros.org/mailman/listinfo/ros-users
>>
> _______________________________________________
> ros-users mailing list
>
> https://code.ros.org/mailman/listinfo/ros-users
>

#include <ros/ros.h>
#include <ros/time.h>

#include <image_transport/image_transport.h>
#include <compressed_image_transport/compressed_publisher.h>

#define BUFSIZE 1024

int main(int argc, char *argv[])
{

        ros::init(argc, argv, "jpeg_publisher");
        ros::NodeHandle nh;
        image_transport::ImageTransport it(nh);
        ros::Publisher pub;


        pub = nh.advertise<sensor_msgs::CompressedImage>("jpeg/image/compressed", 1);


    // read the jpeg file
        FILE *jpeg_pic = fopen("/home/rene/320x240.jpg", "rb");
        long length = 9235; // size of jpeg file in bytes
        char imageBuf[240*BUFSIZE];
        if (fread(imageBuf, length, 1, jpeg_pic) != 1)
    {
        ROS_ERROR("Could not read jpeg file");
        return 0;
    }
        fclose(jpeg_pic);


    // continuously publish the jpeg picture
        while (nh.ok()) {
            sensor_msgs::CompressedImage msg;
                msg.header.stamp = ros::Time::now();
                msg.format = "jpeg";
                msg.data.resize(length);
                memcpy(&msg.data[0], imageBuf, length);
                pub.publish(msg);
        ROS_INFO("published");
        }
        return 0;
}