cloud_test.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 
3 #include <limits>
4 
5 #include "sensor_msgs/PointCloud.h"
6 
8 
9 int main( int argc, char** argv )
10 {
11  ros::init( argc, argv, "cloud_test" );
12 
14 
15  ros::Publisher rgb_pub = n.advertise<sensor_msgs::PointCloud>( "rgb_cloud_test", 0 );
16  ros::Publisher rgb2_pub = n.advertise<sensor_msgs::PointCloud>( "rgb_cloud_test2", 0 );
17  ros::Publisher intensity_pub = n.advertise<sensor_msgs::PointCloud>( "intensity_cloud_test", 0 );
18  ros::Publisher million_pub = n.advertise<sensor_msgs::PointCloud>( "million_points_cloud_test", 0 );
19  ros::Publisher changing_channels_pub = n.advertise<sensor_msgs::PointCloud>( "changing_channels_test", 0 );
20 
21  tf::TransformBroadcaster tf_broadcaster;
22 
23  ros::Duration(0.1).sleep();
24 
25  int i = 0;
26  while (n.ok())
27  {
29  tf::Transform t;
30  t.setIdentity();
31  // tf_broadcaster.sendTransform(tf::Stamped<tf::Transform>(t, tm, "base", "map"));
32 
33  ROS_INFO("Publishing");
34 
35  sensor_msgs::PointCloud changing_cloud;
36 
37  {
38  static sensor_msgs::PointCloud cloud;
39 
40  if (cloud.channels.empty())
41  {
42  cloud.header.stamp = tm;
43  cloud.header.frame_id = "/base_link";
44 
45  cloud.channels.resize(1);
46  int32_t xcount = 100;
47  int32_t ycount = 100;
48  int32_t zcount = 100;
49  int32_t total = xcount * ycount * zcount;
50  cloud.points.resize(total);
51  cloud.channels[0].values.resize(total);
52  cloud.channels[0].name = "intensities";
53  float factor = 0.1f;
54  for (int32_t x = 0; x < xcount; ++x)
55  {
56  for (int32_t y = 0; y < ycount; ++y)
57  {
58  for (int32_t z = 0; z < zcount; ++z)
59  {
60  int32_t index = (ycount*zcount*x) + zcount*y + z;
61  geometry_msgs::Point32& point = cloud.points[index];
62  point.x = x * factor;
63  point.y = y * factor;
64  point.z = z * factor;
65 
66  cloud.channels[0].values[index] = (index % 4096);
67  }
68  }
69  }
70  }
71 
72  million_pub.publish( cloud );
73  }
74 
75  {
76  sensor_msgs::PointCloud cloud;
77  cloud.header.stamp = tm;
78  cloud.header.frame_id = "/base_link";
79 
80  cloud.points.resize(5);
81  cloud.channels.resize(2);
82  for ( int j = 0; j < 5; ++j )
83  {
84  cloud.points[j].x = (float)j;
85  cloud.points[j].y = 0.0f;
86  cloud.points[j].z = i % 10;
87 
88  if (j == 2)
89  {
90  cloud.points[j].z = std::numeric_limits<float>::quiet_NaN();
91  }
92  }
93 
94  cloud.channels[0].name = "rgb";
95  cloud.channels[0].values.resize(5);
96 
97  int rgb = (0xff << 16);
98  cloud.channels[0].values[0] = *reinterpret_cast<float*>(&rgb);
99  rgb = (0xff << 8);
100  cloud.channels[0].values[1] = *reinterpret_cast<float*>(&rgb);
101  rgb = 0xff;
102  cloud.channels[0].values[2] = *reinterpret_cast<float*>(&rgb);
103  rgb = (0xff << 16) | (0xff << 8);
104  cloud.channels[0].values[3] = *reinterpret_cast<float*>(&rgb);
105  rgb = (0xff << 8) | 0xff;
106  cloud.channels[0].values[4] = *reinterpret_cast<float*>(&rgb);
107 
108  cloud.channels[1].name = "intensity";
109  cloud.channels[1].values.resize(5);
110  cloud.channels[1].values[0] = 0;
111  cloud.channels[1].values[1] = 100;
112  cloud.channels[1].values[2] = 200;
113  cloud.channels[1].values[3] = 300;
114  cloud.channels[1].values[4] = 400;
115 
116  rgb_pub.publish(cloud);
117  }
118 
119  {
120  sensor_msgs::PointCloud cloud;
121  cloud.header.stamp = tm;
122  cloud.header.frame_id = "/base_link";
123 
124  cloud.points.resize(5);
125  cloud.channels.resize(3);
126  for ( int j = 0; j < 5; ++j )
127  {
128  cloud.points[j].x = (float)j;
129  cloud.points[j].y = 1.0f;
130  cloud.points[j].z = i % 10;
131  }
132 
133  cloud.channels[0].name = "r";
134  cloud.channels[0].values.resize(5);
135  cloud.channels[1].name = "g";
136  cloud.channels[1].values.resize(5);
137  cloud.channels[2].name = "b";
138  cloud.channels[2].values.resize(5);
139 
140  cloud.channels[0].values[0] = 1.0f;
141  cloud.channels[1].values[0] = 0.0f;
142  cloud.channels[2].values[0] = 0.0f;
143 
144  cloud.channels[0].values[1] = 0.0f;
145  cloud.channels[1].values[1] = 1.0f;
146  cloud.channels[2].values[1] = 0.0f;
147 
148  cloud.channels[0].values[2] = 0.0f;
149  cloud.channels[1].values[2] = 0.0f;
150  cloud.channels[2].values[2] = 1.0f;
151 
152  cloud.channels[0].values[3] = 1.0f;
153  cloud.channels[1].values[3] = 1.0f;
154  cloud.channels[2].values[3] = 0.0f;
155 
156  cloud.channels[0].values[4] = 0.0f;
157  cloud.channels[1].values[4] = 1.0f;
158  cloud.channels[2].values[4] = 1.0f;
159 
160  rgb2_pub.publish(cloud);
161 
162  if ((i % 10) - 5 < 0)
163  {
164  changing_cloud = cloud;
165  }
166  }
167 
168  {
169  sensor_msgs::PointCloud cloud;
170  cloud.header.stamp = tm;
171  cloud.header.frame_id = "/base_link";
172 
173  int num_rows = 1;
174  int num_cols = 200;
175  int total_pts = num_rows * num_cols;
176  cloud.points.resize(total_pts);
177  cloud.channels.resize(1);
178  cloud.channels[0].values.resize(total_pts);
179  cloud.channels[0].name = "intensities";
180 
181  int j = 0;
182  for (int z = 0; z < num_rows; ++z)
183  {
184  for (int x = 0; x < num_cols; ++x, ++j)
185  {
186  cloud.points[j].x = x;
187  cloud.points[j].y = 0;
188 
189  if (num_rows == 1)
190  {
191  cloud.points[j].z = i % 10;
192  }
193  else
194  {
195  cloud.points[j].z = z;
196  }
197 
198  cloud.channels[0].values[j] = j;
199  }
200  }
201 
202  intensity_pub.publish(cloud);
203 
204  if ((i % 10) - 5 >= 0)
205  {
206  changing_cloud = cloud;
207  }
208  }
209 
210  changing_channels_pub.publish(changing_cloud);
211 
212  ++i;
213 
214  ros::Duration(1.0).sleep();
215  }
216 }
void publish(const boost::shared_ptr< M > &message) const
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void setIdentity()
#define ROS_INFO(...)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
Definition: cloud_test.cpp:9
TFSIMD_FORCE_INLINE const tfScalar & z() const
static Time now()
bool ok() const


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:41