171 {
172#ifdef ENABLE_ROS_MSG
173 auto in_msg = std::get<0>(in.values);
174 auto out_msg = std::get<0>(out.values);
175 float x, y, z;
176 float intensity = -1.0;
178 auto& cloud_msg = (*in_msg);
179
180 bool has_x = false, has_y = false, has_z = false, has_intensity = false,
181 has_time = false;
182 std::string time_field_name;
183 sensor_msgs::msg::PointField time_field_type;
184 sensor_msgs::msg::PointField intensity_field_type;
185 for (const auto& field : cloud_msg.fields) {
186 if (field.name == "x") has_x = true;
187 if (field.name == "y") has_y = true;
188 if (field.name == "z") has_z = true;
189 if (field.name == "intensity") {
190 has_intensity = true;
191 intensity_field_type = field;
192 }
193 if (field.name == "t" || field.name == "time" ||
194 field.name == "timestamp") {
195 has_time = true;
196 time_field_type = field;
197 time_field_name = field.name;
198 }
199 }
200 if (!(has_x && has_y && has_z)) {
201 AERROR <<
"PointCloud2 does not contain x, y, z fields.";
202 return false;
203 }
204
205 sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud_msg, "x");
206 sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud_msg, "y");
207 sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud_msg, "z");
208
209 for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
210 x = *iter_x;
211 y = *iter_y;
212 z = *iter_z;
213
214 auto pointcloud = out_msg->add_point();
215 pointcloud->set_x(x);
216 pointcloud->set_y(y);
217 pointcloud->set_z(z);
218 }
219 if (has_time) {
220 ParseField(time_field_type, out_msg, time_field_name, cloud_msg);
221 }
222 if (has_intensity) {
223 ParseField(intensity_field_type, out_msg, "intensity", cloud_msg);
224 }
225#endif
226 return true;
227}