Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <string>
00031 #include "stdlib.h"
00032 #include "ros/ros.h"
00033 #include "sensor_msgs/Image.h"
00034 #include "sensor_msgs/image_encodings.h"
00035 #include "math.h"
00036 #include "image_transport/image_transport.h"
00037
00038 int main( int argc, char **argv )
00039 {
00040 ros::init( argc, argv, "send_images" );
00041
00042 if( argc != 2 )
00043 {
00044 printf( "USAGE: %s <image_format>\n"
00045 " Where <image_format> is rgb8, 32FC1, or 16UC1.",
00046 argv[ 0 ] );
00047 exit( 1 );
00048 }
00049 const std::string image_format( argv[ 1 ]);
00050
00051 ros::NodeHandle nh;
00052 image_transport::ImageTransport it( nh );
00053 image_transport::Publisher pub = it.advertise("images", 100);
00054 ros::Rate loop_rate( 100 );
00055
00056 if( image_format == "rgb8" )
00057 {
00058 sensor_msgs::Image msg;
00059 int width = 100;
00060 int height = 1000;
00061 msg.data.resize( width * height * 3 );
00062 msg.header.frame_id = "base_link";
00063 msg.height = height;
00064 msg.width = width;
00065 msg.encoding = image_format;
00066 msg.step = width * 3;
00067
00068 int count = 0;
00069 while( ros::ok() )
00070 {
00071 for( int x = 0; x < width; x++ )
00072 {
00073 for( int y = 0; y < height; y++ )
00074 {
00075 int index = (x + y * width) * 3;
00076 long int rand = random();
00077 msg.data[ index ] = rand & 0xff;
00078 index++;
00079 msg.data[ index ] = (rand >> 8) & 0xff;
00080 index++;
00081 msg.data[ index ] = (rand >> 16) & 0xff;
00082 }
00083 }
00084 msg.header.seq = count;
00085 msg.header.stamp = ros::Time::now();
00086
00087 pub.publish( msg );
00088
00089 ros::spinOnce();
00090 loop_rate.sleep();
00091 ++count;
00092 }
00093 }
00094 else if( image_format == "32FC1" )
00095 {
00096 sensor_msgs::Image msg;
00097 int width = 400;
00098 int height = 400;
00099 msg.data.resize( width * height * sizeof( float ));
00100 msg.header.frame_id = "base_link";
00101 msg.height = height;
00102 msg.width = width;
00103 msg.encoding = image_format;
00104 msg.step = width;
00105
00106 int count = 0;
00107 while( ros::ok() )
00108 {
00109 for( int x = 0; x < width; x++ )
00110 {
00111 for( int y = 0; y < height; y++ )
00112 {
00113 int index = x + y * width;
00114 float* ptr = ((float*) &msg.data[ 0 ]) + index;
00115 *ptr = sinf( (x + count) / 10.0f ) * sinf( y / 10.0f ) * 20.0f - 10.0f;
00116 }
00117 }
00118 msg.header.seq = count;
00119 msg.header.stamp = ros::Time::now();
00120
00121 pub.publish( msg );
00122
00123 ros::spinOnce();
00124 loop_rate.sleep();
00125 ++count;
00126 }
00127 }
00128 else if( image_format == "16UC1" )
00129 {
00130 sensor_msgs::Image msg;
00131 int width = 400;
00132 int height = 400;
00133 msg.data.resize( width * height * sizeof( short ));
00134 msg.header.frame_id = "base_link";
00135 msg.height = height;
00136 msg.width = width;
00137 msg.encoding = image_format;
00138 msg.step = width;
00139
00140 int count = 0;
00141 while( ros::ok() )
00142 {
00143 for( int x = 0; x < width; x++ )
00144 {
00145 for( int y = 0; y < height; y++ )
00146 {
00147 int index = x + y * width;
00148 short* ptr = ((short*) &msg.data[ 0 ]) + index;
00149 *ptr = (count + abs( x % 100 - 50 ) + abs( y % 100 - 50 )) % 50 * 65535 / 50;
00150 }
00151 }
00152 msg.header.seq = count;
00153 msg.header.stamp = ros::Time::now();
00154
00155 pub.publish( msg );
00156
00157 ros::spinOnce();
00158 loop_rate.sleep();
00159 ++count;
00160 }
00161 }
00162 }