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/PointCloud2.h"
00032 #include "math.h"
00033
00034 int main( int argc, char **argv )
00035 {
00036 ros::init( argc, argv, "send_points2" );
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::PointCloud2>("points2", 10);
00058 ros::Rate loop_rate( rate );
00059
00060 sensor_msgs::PointCloud2 msg;
00061 int width = size;
00062 int height = 2*size;
00063 msg.header.frame_id = "base_link";
00064 msg.is_dense = false;
00065 msg.is_bigendian = false;
00066
00067 msg.fields.resize( 5 );
00068
00069 msg.fields[0].name = "x";
00070 msg.fields[0].offset = 0;
00071 msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00072 msg.fields[0].count = 1;
00073
00074 msg.fields[1].name = "y";
00075 msg.fields[1].offset = 4;
00076 msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00077 msg.fields[1].count = 1;
00078
00079 msg.fields[2].name = "z";
00080 msg.fields[2].offset = 8;
00081 msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00082 msg.fields[2].count = 1;
00083
00084 msg.fields[3].name = "rgb";
00085 msg.fields[3].offset = 12;
00086 msg.fields[3].datatype = sensor_msgs::PointField::INT32;
00087 msg.fields[3].count = 1;
00088
00089 msg.fields[4].name = "joy";
00090 msg.fields[4].offset = 16;
00091 msg.fields[4].datatype = sensor_msgs::PointField::FLOAT32;
00092 msg.fields[4].count = 1;
00093
00094 msg.point_step = 20;
00095
00096 int count = 0;
00097 while( ros::ok() )
00098 {
00099 width++;
00100 int num_points = width * height;
00101 msg.row_step = width * msg.point_step;
00102 msg.height = height;
00103 msg.width = width;
00104 msg.data.resize( num_points * msg.point_step );
00105 for( int x = 0; x < width; x++ )
00106 {
00107 for( int y = 0; y < height; y++ )
00108 {
00109 uint8_t* ptr = &msg.data[0] + (x + y * width) * msg.point_step;
00110 *(float*)ptr = x / 100.0f;
00111 ptr += 4;
00112 *(float*)ptr = y / 100.0f;
00113 ptr += 4;
00114 *(float*)ptr = 0.1 * sinf( x / 10.0f ) * sinf( y / 10.0f );
00115 ptr += 4;
00116 *ptr = (x+count) & 0xff;
00117 ptr++;
00118 *ptr = y & 0xff;
00119 ptr++;
00120 *ptr = (x+y) & 0xff;
00121 ptr++;
00122 ptr++;
00123 *(float*)ptr = 127.0f + 127.0f * sinf( (x - count)/ 10.0f ) * sinf( y / 10.0f );
00124
00125 }
00126 }
00127 msg.header.seq = count;
00128 msg.header.stamp = ros::Time::now();
00129
00130 printf( "publishing at %d hz, %s, %d x %d points.\n",
00131 rate, (moving?"moving":"static"), width, height );
00132
00133 pub.publish( msg );
00134
00135 ros::spinOnce();
00136 loop_rate.sleep();
00137 if( moving )
00138 {
00139 ++count;
00140 }
00141 }
00142 }