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