31 #include "sensor_msgs/PointCloud.h" 34 int main(
int argc,
char **argv )
36 ros::init( argc, argv,
"send_lots_of_points" );
44 rate = atoi( argv[1] );
48 moving = bool( atoi( argv[2] ));
52 size = atoi( argv[3] );
60 sensor_msgs::PointCloud msg;
63 msg.points.resize( width * length );
64 msg.header.frame_id =
"base_link";
70 msg.points.resize( width * length + (count % 2) );
72 for(
int x = 0;
x < width;
x++ )
74 for(
int y = 0;
y < length;
y++ )
76 geometry_msgs::Point32 & point = msg.points[
x +
y * width ];
77 point.x = float(
x / 100.0);
78 point.y = float(
y / 100.0);
80 point.z = ((
x +
y + count) % 100) / 100.0;
85 msg.points[ width * length + 1 ].x = -.1;
86 msg.points[ width * length + 1 ].y = -.1;
87 msg.points[ width * length + 1 ].z = 1.1;
89 msg.header.seq = count;
92 printf(
"publishing at %d hz, %s, %d x %d points.\n",
93 rate, (moving?
"moving":
"static"), width, length );
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
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
ROSCPP_DECL void spinOnce()