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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Fri Aug 12 2022 02:06:08