send_lots_of_points_node.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/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 //        point.z = sinf( x / 100.0 + y / 100.0 + count / 100.0 );
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 }


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32