31 #include "sensor_msgs/PointCloud2.h" 34 int main(
int argc,
char **argv )
44 rate = atoi( argv[1] );
48 moving = bool( atoi( argv[2] ));
52 size = atoi( argv[3] );
60 sensor_msgs::PointCloud2 msg;
63 msg.header.frame_id =
"base_link";
65 msg.is_bigendian =
false;
67 msg.fields.resize( 5 );
69 msg.fields[0].name =
"x";
70 msg.fields[0].offset = 0;
71 msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
72 msg.fields[0].count = 1;
74 msg.fields[1].name =
"y";
75 msg.fields[1].offset = 4;
76 msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
77 msg.fields[1].count = 1;
79 msg.fields[2].name =
"z";
80 msg.fields[2].offset = 8;
81 msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
82 msg.fields[2].count = 1;
84 msg.fields[3].name =
"rgb";
85 msg.fields[3].offset = 12;
86 msg.fields[3].datatype = sensor_msgs::PointField::INT32;
87 msg.fields[3].count = 1;
89 msg.fields[4].name =
"joy";
90 msg.fields[4].offset = 16;
91 msg.fields[4].datatype = sensor_msgs::PointField::FLOAT32;
92 msg.fields[4].count = 1;
100 int num_points = width * height;
101 msg.row_step = width * msg.point_step;
104 msg.data.resize( num_points * msg.point_step );
105 for(
int x = 0;
x < width;
x++ )
107 for(
int y = 0;
y < height;
y++ )
109 uint8_t* ptr = &msg.data[0] + (
x +
y * width) * msg.point_step;
110 *(
float*)ptr =
x / 100.0f;
112 *(
float*)ptr =
y / 100.0
f;
114 *(
float*)ptr = 0.1 * sinf(
x / 10.0
f ) * sinf(
y / 10.0
f );
116 *ptr = (
x+count) & 0xff;
123 *(
float*)ptr = 127.0
f + 127.0
f * sinf( (
x - count)/ 10.0f ) * sinf(
y / 10.0
f );
127 msg.header.seq = count;
130 printf(
"publishing at %d hz, %s, %d x %d points.\n",
131 rate, (moving?
"moving":
"static"), width, height );
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spinOnce()