33 #include "sensor_msgs/Image.h" 38 int main(
int argc,
char **argv )
44 printf(
"USAGE: %s <image_format>\n" 45 " Where <image_format> is rgb8, 32FC1, or 16UC1.",
49 const std::string image_format( argv[ 1 ]);
56 if( image_format ==
"rgb8" )
58 sensor_msgs::Image msg;
61 msg.data.resize( width * height * 3 );
62 msg.header.frame_id =
"base_link";
65 msg.encoding = image_format;
71 for(
int x = 0;
x < width;
x++ )
73 for(
int y = 0;
y < height;
y++ )
75 int index = (
x +
y * width) * 3;
76 long int rand = random();
77 msg.data[ index ] = rand & 0xff;
79 msg.data[ index ] = (rand >> 8) & 0xff;
81 msg.data[ index ] = (rand >> 16) & 0xff;
84 msg.header.seq = count;
94 else if( image_format ==
"32FC1" )
96 sensor_msgs::Image msg;
99 msg.data.resize( width * height *
sizeof(
float ));
100 msg.header.frame_id =
"base_link";
103 msg.encoding = image_format;
109 for(
int x = 0;
x < width;
x++ )
111 for(
int y = 0;
y < height;
y++ )
113 int index =
x +
y * width;
114 float* ptr = ((
float*) &msg.data[ 0 ]) + index;
115 *ptr = sinf( (
x + count) / 10.0
f ) * sinf(
y / 10.0
f ) * 20.0f - 10.0f;
118 msg.header.seq = count;
128 else if( image_format ==
"16UC1" )
130 sensor_msgs::Image msg;
133 msg.data.resize( width * height *
sizeof(
short ));
134 msg.header.frame_id =
"base_link";
137 msg.encoding = image_format;
143 for(
int x = 0;
x < width;
x++ )
145 for(
int y = 0;
y < height;
y++ )
147 int index =
x +
y * width;
148 short* ptr = ((
short*) &msg.data[ 0 ]) + index;
149 *ptr = (count + abs(
x % 100 - 50 ) + abs(
y % 100 - 50 )) % 50 * 65535 / 50;
152 msg.header.seq = count;
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void publish(const sensor_msgs::Image &message) const
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & x() const
ROSCPP_DECL void spinOnce()