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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:09