5 #include "sensor_msgs/PointCloud.h" 9 int main(
int argc,
char** argv )
35 sensor_msgs::PointCloud changing_cloud;
38 static sensor_msgs::PointCloud cloud;
40 if (cloud.channels.empty())
42 cloud.header.stamp = tm;
43 cloud.header.frame_id =
"/base_link";
45 cloud.channels.resize(1);
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";
54 for (int32_t
x = 0;
x < xcount; ++
x)
56 for (int32_t
y = 0;
y < ycount; ++
y)
58 for (int32_t
z = 0;
z < zcount; ++
z)
60 int32_t index = (ycount*zcount*
x) + zcount*
y +
z;
61 geometry_msgs::Point32& point = cloud.points[index];
66 cloud.channels[0].values[index] = (index % 4096);
72 million_pub.publish( cloud );
76 sensor_msgs::PointCloud cloud;
77 cloud.header.stamp = tm;
78 cloud.header.frame_id =
"/base_link";
80 cloud.points.resize(5);
81 cloud.channels.resize(2);
82 for (
int j = 0; j < 5; ++j )
84 cloud.points[j].x = (float)j;
85 cloud.points[j].y = 0.0f;
86 cloud.points[j].z = i % 10;
90 cloud.points[j].z = std::numeric_limits<float>::quiet_NaN();
94 cloud.channels[0].name =
"rgb";
95 cloud.channels[0].values.resize(5);
97 int rgb = (0xff << 16);
98 cloud.channels[0].values[0] = *
reinterpret_cast<float*
>(&rgb);
100 cloud.channels[0].values[1] = *
reinterpret_cast<float*
>(&rgb);
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);
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;
120 sensor_msgs::PointCloud cloud;
121 cloud.header.stamp = tm;
122 cloud.header.frame_id =
"/base_link";
124 cloud.points.resize(5);
125 cloud.channels.resize(3);
126 for (
int j = 0; j < 5; ++j )
128 cloud.points[j].x = (float)j;
129 cloud.points[j].y = 1.0f;
130 cloud.points[j].z = i % 10;
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);
140 cloud.channels[0].values[0] = 1.0f;
141 cloud.channels[1].values[0] = 0.0f;
142 cloud.channels[2].values[0] = 0.0f;
144 cloud.channels[0].values[1] = 0.0f;
145 cloud.channels[1].values[1] = 1.0f;
146 cloud.channels[2].values[1] = 0.0f;
148 cloud.channels[0].values[2] = 0.0f;
149 cloud.channels[1].values[2] = 0.0f;
150 cloud.channels[2].values[2] = 1.0f;
152 cloud.channels[0].values[3] = 1.0f;
153 cloud.channels[1].values[3] = 1.0f;
154 cloud.channels[2].values[3] = 0.0f;
156 cloud.channels[0].values[4] = 0.0f;
157 cloud.channels[1].values[4] = 1.0f;
158 cloud.channels[2].values[4] = 1.0f;
160 rgb2_pub.publish(cloud);
162 if ((i % 10) - 5 < 0)
164 changing_cloud = cloud;
169 sensor_msgs::PointCloud cloud;
170 cloud.header.stamp = tm;
171 cloud.header.frame_id =
"/base_link";
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";
182 for (
int z = 0;
z < num_rows; ++
z)
184 for (
int x = 0;
x < num_cols; ++
x, ++j)
186 cloud.points[j].x =
x;
187 cloud.points[j].y = 0;
191 cloud.points[j].z = i % 10;
195 cloud.points[j].z =
z;
198 cloud.channels[0].values[j] = j;
202 intensity_pub.publish(cloud);
204 if ((i % 10) - 5 >= 0)
206 changing_cloud = cloud;
210 changing_channels_pub.publish(changing_cloud);
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & y() const
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)
TFSIMD_FORCE_INLINE const tfScalar & z() const