send_point_cloud_2.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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         // ptr += 4;
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 }


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:28