cloud_test.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 
00003 #include <limits>
00004 
00005 #include "sensor_msgs/PointCloud.h"
00006 
00007 #include <tf/transform_broadcaster.h>
00008 
00009 int main( int argc, char** argv )
00010 {
00011   ros::init( argc, argv, "cloud_test" );
00012 
00013   ros::NodeHandle n;
00014 
00015   ros::Publisher rgb_pub = n.advertise<sensor_msgs::PointCloud>( "rgb_cloud_test", 0 );
00016   ros::Publisher rgb2_pub = n.advertise<sensor_msgs::PointCloud>( "rgb_cloud_test2", 0 );
00017   ros::Publisher intensity_pub = n.advertise<sensor_msgs::PointCloud>( "intensity_cloud_test", 0 );
00018   ros::Publisher million_pub = n.advertise<sensor_msgs::PointCloud>( "million_points_cloud_test", 0 );
00019   ros::Publisher changing_channels_pub = n.advertise<sensor_msgs::PointCloud>( "changing_channels_test", 0 );
00020 
00021   tf::TransformBroadcaster tf_broadcaster;
00022 
00023   ros::Duration(0.1).sleep();
00024 
00025   int i = 0;
00026   while (n.ok())
00027   {
00028     ros::Time tm(ros::Time::now());
00029     tf::Transform t;
00030     t.setIdentity();
00031     //    tf_broadcaster.sendTransform(tf::Stamped<tf::Transform>(t, tm, "base", "map"));
00032 
00033     ROS_INFO("Publishing");
00034 
00035     sensor_msgs::PointCloud changing_cloud;
00036 
00037     {
00038       static sensor_msgs::PointCloud cloud;
00039 
00040       if (cloud.channels.empty())
00041       {
00042         cloud.header.stamp = tm;
00043         cloud.header.frame_id = "/base_link";
00044 
00045         cloud.channels.resize(1);
00046         int32_t xcount = 100;
00047         int32_t ycount = 100;
00048         int32_t zcount = 100;
00049         int32_t total = xcount * ycount * zcount;
00050         cloud.points.resize(total);
00051         cloud.channels[0].values.resize(total);
00052         cloud.channels[0].name = "intensities";
00053         float factor = 0.1f;
00054         for (int32_t x = 0; x < xcount; ++x)
00055         {
00056           for (int32_t y = 0; y < ycount; ++y)
00057           {
00058             for (int32_t z = 0; z < zcount; ++z)
00059             {
00060               int32_t index = (ycount*zcount*x) + zcount*y + z;
00061               geometry_msgs::Point32& point = cloud.points[index];
00062               point.x = x * factor;
00063               point.y = y * factor;
00064               point.z = z * factor;
00065 
00066               cloud.channels[0].values[index] = (index % 4096);
00067             }
00068           }
00069         }
00070       }
00071 
00072       million_pub.publish( cloud );
00073     }
00074 
00075     {
00076       sensor_msgs::PointCloud cloud;
00077       cloud.header.stamp = tm;
00078       cloud.header.frame_id = "/base_link";
00079 
00080       cloud.points.resize(5);
00081       cloud.channels.resize(2);
00082       for ( int j = 0; j < 5; ++j )
00083       {
00084         cloud.points[j].x = (float)j;
00085         cloud.points[j].y = 0.0f;
00086         cloud.points[j].z = i % 10;
00087 
00088         if (j == 2)
00089         {
00090           cloud.points[j].z = std::numeric_limits<float>::quiet_NaN();
00091         }
00092       }
00093 
00094       cloud.channels[0].name = "rgb";
00095       cloud.channels[0].values.resize(5);
00096 
00097       int rgb = (0xff << 16);
00098       cloud.channels[0].values[0] = *reinterpret_cast<float*>(&rgb);
00099       rgb = (0xff << 8);
00100       cloud.channels[0].values[1] = *reinterpret_cast<float*>(&rgb);
00101       rgb = 0xff;
00102       cloud.channels[0].values[2] = *reinterpret_cast<float*>(&rgb);
00103       rgb = (0xff << 16) | (0xff << 8);
00104       cloud.channels[0].values[3] = *reinterpret_cast<float*>(&rgb);
00105       rgb = (0xff << 8) | 0xff;
00106       cloud.channels[0].values[4] = *reinterpret_cast<float*>(&rgb);
00107 
00108       cloud.channels[1].name = "intensity";
00109       cloud.channels[1].values.resize(5);
00110       cloud.channels[1].values[0] = 0;
00111       cloud.channels[1].values[1] = 100;
00112       cloud.channels[1].values[2] = 200;
00113       cloud.channels[1].values[3] = 300;
00114       cloud.channels[1].values[4] = 400;
00115 
00116       rgb_pub.publish(cloud);
00117     }
00118 
00119     {
00120       sensor_msgs::PointCloud cloud;
00121       cloud.header.stamp = tm;
00122       cloud.header.frame_id = "/base_link";
00123 
00124       cloud.points.resize(5);
00125       cloud.channels.resize(3);
00126       for ( int j = 0; j < 5; ++j )
00127       {
00128         cloud.points[j].x = (float)j;
00129         cloud.points[j].y = 1.0f;
00130         cloud.points[j].z = i % 10;
00131       }
00132 
00133       cloud.channels[0].name = "r";
00134       cloud.channels[0].values.resize(5);
00135       cloud.channels[1].name = "g";
00136       cloud.channels[1].values.resize(5);
00137       cloud.channels[2].name = "b";
00138       cloud.channels[2].values.resize(5);
00139 
00140       cloud.channels[0].values[0] = 1.0f;
00141       cloud.channels[1].values[0] = 0.0f;
00142       cloud.channels[2].values[0] = 0.0f;
00143 
00144       cloud.channels[0].values[1] = 0.0f;
00145       cloud.channels[1].values[1] = 1.0f;
00146       cloud.channels[2].values[1] = 0.0f;
00147 
00148       cloud.channels[0].values[2] = 0.0f;
00149       cloud.channels[1].values[2] = 0.0f;
00150       cloud.channels[2].values[2] = 1.0f;
00151 
00152       cloud.channels[0].values[3] = 1.0f;
00153       cloud.channels[1].values[3] = 1.0f;
00154       cloud.channels[2].values[3] = 0.0f;
00155 
00156       cloud.channels[0].values[4] = 0.0f;
00157       cloud.channels[1].values[4] = 1.0f;
00158       cloud.channels[2].values[4] = 1.0f;
00159 
00160       rgb2_pub.publish(cloud);
00161 
00162       if ((i % 10) - 5 < 0)
00163       {
00164         changing_cloud = cloud;
00165       }
00166     }
00167 
00168     {
00169       sensor_msgs::PointCloud cloud;
00170       cloud.header.stamp = tm;
00171       cloud.header.frame_id = "/base_link";
00172 
00173       int num_rows = 1;
00174       int num_cols = 200;
00175       int total_pts = num_rows * num_cols;
00176       cloud.points.resize(total_pts);
00177       cloud.channels.resize(1);
00178       cloud.channels[0].values.resize(total_pts);
00179       cloud.channels[0].name = "intensities";
00180 
00181       int j = 0;
00182       for (int z = 0; z < num_rows; ++z)
00183       {
00184         for (int x = 0; x < num_cols; ++x, ++j)
00185         {
00186           cloud.points[j].x = x;
00187           cloud.points[j].y = 0;
00188 
00189           if (num_rows == 1)
00190           {
00191             cloud.points[j].z = i % 10;
00192           }
00193           else
00194           {
00195             cloud.points[j].z = z;
00196           }
00197 
00198           cloud.channels[0].values[j] = j;
00199         }
00200       }
00201 
00202       intensity_pub.publish(cloud);
00203 
00204       if ((i % 10) - 5 >= 0)
00205       {
00206         changing_cloud = cloud;
00207       }
00208     }
00209 
00210     changing_channels_pub.publish(changing_cloud);
00211 
00212     ++i;
00213 
00214     ros::Duration(1.0).sleep();
00215   }
00216 }


rviz_qt
Author(s): Dave Hershberger
autogenerated on Fri Dec 6 2013 20:56:52