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 "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 }