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 "ros/ros.h"
00031 #include "sensor_msgs/PointCloud.h"
00032 #include "math.h"
00033
00034 int main( int argc, char **argv )
00035 {
00036 ros::init( argc, argv, "send_lots_of_points" );
00037
00038 int rate = 1;
00039 bool moving = true;
00040 int size = 100;
00041
00042 if( argc > 1 )
00043 {
00044 rate = atoi( argv[1] );
00045 }
00046 if( argc > 2 )
00047 {
00048 moving = bool( atoi( argv[2] ));
00049 }
00050 if( argc > 3 )
00051 {
00052 size = atoi( argv[3] );
00053 }
00054
00055 ros::NodeHandle nh;
00056
00057 ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud>("lots_of_points", 100);
00058 ros::Rate loop_rate( rate );
00059
00060 sensor_msgs::PointCloud msg;
00061 int width = size;
00062 int length = 2*size;
00063 msg.points.resize( width * length );
00064 msg.header.frame_id = "base_link";
00065
00066 printf( "publishing at %d hz, %s, %d x %d points.\n",
00067 rate, (moving?"moving":"static"), width, length );
00068
00069 int count = 0;
00070 while( ros::ok() )
00071 {
00072 msg.points.resize( width * length + (count % 2) );
00073
00074 for( int x = 0; x < width; x++ )
00075 {
00076 for( int y = 0; y < length; y++ )
00077 {
00078 geometry_msgs::Point32 & point = msg.points[ x + y * width ];
00079 point.x = float(x / 100.0);
00080 point.y = float(y / 100.0);
00081
00082 point.z = ((x + y + count) % 100) / 100.0;
00083 }
00084 }
00085 if( count % 2 )
00086 {
00087 msg.points[ width * length + 1 ].x = -.1;
00088 msg.points[ width * length + 1 ].y = -.1;
00089 msg.points[ width * length + 1 ].z = 1.1;
00090 }
00091 msg.header.seq = count;
00092 msg.header.stamp = ros::Time::now();
00093
00094 pub.publish( msg );
00095
00096 ros::spinOnce();
00097 loop_rate.sleep();
00098 if( moving )
00099 {
00100 ++count;
00101 }
00102 }
00103 }