@@ -230,8 +230,9 @@ void CropBoxFilter::filter_pointcloud(const PointCloud2ConstPtr & cloud, PointCl
230230void CropBoxFilter::pointcloud_callback (const PointCloud2ConstPtr cloud)
231231{
232232 // check whether the pointcloud is valid
233- if (!is_valid (cloud)) {
234- RCLCPP_ERROR (this ->get_logger (), " [input_pointcloud_callback] Invalid input pointcloud!" );
233+ const ValidationResult result = validate_pointcloud2 (*cloud);
234+ if (!result.is_valid ) {
235+ RCLCPP_ERROR (this ->get_logger (), " [input_pointcloud_callback] %s" , result.reason .c_str ());
235236 return ;
236237 }
237238
@@ -356,236 +357,6 @@ rcl_interfaces::msg::SetParametersResult CropBoxFilter::param_callback(
356357 return result;
357358}
358359
359- bool CropBoxFilter::is_data_layout_compatible_with_point_xyzi (const PointCloud2 & input)
360- {
361- using PointIndex = autoware::point_types::PointXYZIIndex;
362- using autoware::point_types::PointXYZI;
363- if (input.fields .size () < 4 ) {
364- return false ;
365- }
366- bool same_layout = true ;
367- const auto & field_x = input.fields .at (static_cast <size_t >(PointIndex::X));
368- same_layout &= field_x.name == " x" ;
369- same_layout &= field_x.offset == offsetof (PointXYZI, x);
370- same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
371- same_layout &= field_x.count == 1 ;
372- const auto & field_y = input.fields .at (static_cast <size_t >(PointIndex::Y));
373- same_layout &= field_y.name == " y" ;
374- same_layout &= field_y.offset == offsetof (PointXYZI, y);
375- same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
376- same_layout &= field_y.count == 1 ;
377- const auto & field_z = input.fields .at (static_cast <size_t >(PointIndex::Z));
378- same_layout &= field_z.name == " z" ;
379- same_layout &= field_z.offset == offsetof (PointXYZI, z);
380- same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
381- same_layout &= field_z.count == 1 ;
382- const auto & field_intensity = input.fields .at (static_cast <size_t >(PointIndex::Intensity));
383- same_layout &= field_intensity.name == " intensity" ;
384- same_layout &= field_intensity.offset == offsetof (PointXYZI, intensity);
385- same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32;
386- same_layout &= field_intensity.count == 1 ;
387- return same_layout;
388- }
389-
390- bool CropBoxFilter::is_data_layout_compatible_with_point_xyzirc (const PointCloud2 & input)
391- {
392- using PointIndex = autoware::point_types::PointXYZIRCIndex;
393- using autoware::point_types::PointXYZIRC;
394- if (input.fields .size () < 6 ) {
395- return false ;
396- }
397- bool same_layout = true ;
398- const auto & field_x = input.fields .at (static_cast <size_t >(PointIndex::X));
399- same_layout &= field_x.name == " x" ;
400- same_layout &= field_x.offset == offsetof (PointXYZIRC, x);
401- same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
402- same_layout &= field_x.count == 1 ;
403- const auto & field_y = input.fields .at (static_cast <size_t >(PointIndex::Y));
404- same_layout &= field_y.name == " y" ;
405- same_layout &= field_y.offset == offsetof (PointXYZIRC, y);
406- same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
407- same_layout &= field_y.count == 1 ;
408- const auto & field_z = input.fields .at (static_cast <size_t >(PointIndex::Z));
409- same_layout &= field_z.name == " z" ;
410- same_layout &= field_z.offset == offsetof (PointXYZIRC, z);
411- same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
412- same_layout &= field_z.count == 1 ;
413- const auto & field_intensity = input.fields .at (static_cast <size_t >(PointIndex::Intensity));
414- same_layout &= field_intensity.name == " intensity" ;
415- same_layout &= field_intensity.offset == offsetof (PointXYZIRC, intensity);
416- same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8;
417- same_layout &= field_intensity.count == 1 ;
418- const auto & field_return_type = input.fields .at (static_cast <size_t >(PointIndex::ReturnType));
419- same_layout &= field_return_type.name == " return_type" ;
420- same_layout &= field_return_type.offset == offsetof (PointXYZIRC, return_type);
421- same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8;
422- same_layout &= field_return_type.count == 1 ;
423- const auto & field_ring = input.fields .at (static_cast <size_t >(PointIndex::Channel));
424- same_layout &= field_ring.name == " channel" ;
425- same_layout &= field_ring.offset == offsetof (PointXYZIRC, channel);
426- same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16;
427- same_layout &= field_ring.count == 1 ;
428-
429- return same_layout;
430- }
431-
432- bool CropBoxFilter::is_data_layout_compatible_with_point_xyziradrt (const PointCloud2 & input)
433- {
434- using PointIndex = autoware::point_types::PointXYZIRADRTIndex;
435- using autoware::point_types::PointXYZIRADRT;
436- if (input.fields .size () < 9 ) {
437- return false ;
438- }
439- bool same_layout = true ;
440- const auto & field_x = input.fields .at (static_cast <size_t >(PointIndex::X));
441- same_layout &= field_x.name == " x" ;
442- same_layout &= field_x.offset == offsetof (PointXYZIRADRT, x);
443- same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
444- same_layout &= field_x.count == 1 ;
445- const auto & field_y = input.fields .at (static_cast <size_t >(PointIndex::Y));
446- same_layout &= field_y.name == " y" ;
447- same_layout &= field_y.offset == offsetof (PointXYZIRADRT, y);
448- same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
449- same_layout &= field_y.count == 1 ;
450- const auto & field_z = input.fields .at (static_cast <size_t >(PointIndex::Z));
451- same_layout &= field_z.name == " z" ;
452- same_layout &= field_z.offset == offsetof (PointXYZIRADRT, z);
453- same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
454- same_layout &= field_z.count == 1 ;
455- const auto & field_intensity = input.fields .at (static_cast <size_t >(PointIndex::Intensity));
456- same_layout &= field_intensity.name == " intensity" ;
457- same_layout &= field_intensity.offset == offsetof (PointXYZIRADRT, intensity);
458- same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32;
459- same_layout &= field_intensity.count == 1 ;
460- const auto & field_ring = input.fields .at (static_cast <size_t >(PointIndex::Ring));
461- same_layout &= field_ring.name == " ring" ;
462- same_layout &= field_ring.offset == offsetof (PointXYZIRADRT, ring);
463- same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16;
464- same_layout &= field_ring.count == 1 ;
465- const auto & field_azimuth = input.fields .at (static_cast <size_t >(PointIndex::Azimuth));
466- same_layout &= field_azimuth.name == " azimuth" ;
467- same_layout &= field_azimuth.offset == offsetof (PointXYZIRADRT, azimuth);
468- same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32;
469- same_layout &= field_azimuth.count == 1 ;
470- const auto & field_distance = input.fields .at (static_cast <size_t >(PointIndex::Distance));
471- same_layout &= field_distance.name == " distance" ;
472- same_layout &= field_distance.offset == offsetof (PointXYZIRADRT, distance);
473- same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32;
474- same_layout &= field_distance.count == 1 ;
475- const auto & field_return_type = input.fields .at (static_cast <size_t >(PointIndex::ReturnType));
476- same_layout &= field_return_type.name == " return_type" ;
477- same_layout &= field_return_type.offset == offsetof (PointXYZIRADRT, return_type);
478- same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8;
479- same_layout &= field_return_type.count == 1 ;
480- const auto & field_time_stamp = input.fields .at (static_cast <size_t >(PointIndex::TimeStamp));
481- same_layout &= field_time_stamp.name == " time_stamp" ;
482- same_layout &= field_time_stamp.offset == offsetof (PointXYZIRADRT, time_stamp);
483- same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64;
484- same_layout &= field_time_stamp.count == 1 ;
485- return same_layout;
486- }
487-
488- bool CropBoxFilter::is_data_layout_compatible_with_point_xyzircaedt (const PointCloud2 & input)
489- {
490- using PointIndex = autoware::point_types::PointXYZIRCAEDTIndex;
491- using autoware::point_types::PointXYZIRCAEDT;
492- if (input.fields .size () != 10 ) {
493- return false ;
494- }
495- bool same_layout = true ;
496- const auto & field_x = input.fields .at (static_cast <size_t >(PointIndex::X));
497- same_layout &= field_x.name == " x" ;
498- same_layout &= field_x.offset == offsetof (PointXYZIRCAEDT, x);
499- same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
500- same_layout &= field_x.count == 1 ;
501- const auto & field_y = input.fields .at (static_cast <size_t >(PointIndex::Y));
502- same_layout &= field_y.name == " y" ;
503- same_layout &= field_y.offset == offsetof (PointXYZIRCAEDT, y);
504- same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
505- same_layout &= field_y.count == 1 ;
506- const auto & field_z = input.fields .at (static_cast <size_t >(PointIndex::Z));
507- same_layout &= field_z.name == " z" ;
508- same_layout &= field_z.offset == offsetof (PointXYZIRCAEDT, z);
509- same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
510- same_layout &= field_z.count == 1 ;
511- const auto & field_intensity = input.fields .at (static_cast <size_t >(PointIndex::Intensity));
512- same_layout &= field_intensity.name == " intensity" ;
513- same_layout &= field_intensity.offset == offsetof (PointXYZIRCAEDT, intensity);
514- same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8;
515- same_layout &= field_intensity.count == 1 ;
516- const auto & field_return_type = input.fields .at (static_cast <size_t >(PointIndex::ReturnType));
517- same_layout &= field_return_type.name == " return_type" ;
518- same_layout &= field_return_type.offset == offsetof (PointXYZIRCAEDT, return_type);
519- same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8;
520- same_layout &= field_return_type.count == 1 ;
521- const auto & field_ring = input.fields .at (static_cast <size_t >(PointIndex::Channel));
522- same_layout &= field_ring.name == " channel" ;
523- same_layout &= field_ring.offset == offsetof (PointXYZIRCAEDT, channel);
524- same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16;
525- same_layout &= field_ring.count == 1 ;
526- const auto & field_azimuth = input.fields .at (static_cast <size_t >(PointIndex::Azimuth));
527- same_layout &= field_azimuth.name == " azimuth" ;
528- same_layout &= field_azimuth.offset == offsetof (PointXYZIRCAEDT, azimuth);
529- same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32;
530- same_layout &= field_azimuth.count == 1 ;
531- const auto & field_elevation = input.fields .at (static_cast <size_t >(PointIndex::Elevation));
532- same_layout &= field_elevation.name == " elevation" ;
533- same_layout &= field_elevation.offset == offsetof (PointXYZIRCAEDT, elevation);
534- same_layout &= field_elevation.datatype == sensor_msgs::msg::PointField::FLOAT32;
535- same_layout &= field_elevation.count == 1 ;
536- const auto & field_distance = input.fields .at (static_cast <size_t >(PointIndex::Distance));
537- same_layout &= field_distance.name == " distance" ;
538- same_layout &= field_distance.offset == offsetof (PointXYZIRCAEDT, distance);
539- same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32;
540- same_layout &= field_distance.count == 1 ;
541- const auto & field_time_stamp = input.fields .at (static_cast <size_t >(PointIndex::TimeStamp));
542- same_layout &= field_time_stamp.name == " time_stamp" ;
543- same_layout &= field_time_stamp.offset == offsetof (PointXYZIRCAEDT, time_stamp);
544- same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::UINT32;
545- same_layout &= field_time_stamp.count == 1 ;
546- return same_layout;
547- }
548-
549- bool CropBoxFilter::is_valid (const PointCloud2ConstPtr & cloud)
550- {
551- // firstly check the fields of the point cloud
552- if (
553- !is_data_layout_compatible_with_point_xyzircaedt (*cloud) &&
554- !is_data_layout_compatible_with_point_xyzirc (*cloud)) {
555- RCLCPP_ERROR (
556- get_logger (),
557- " The pointcloud layout is not compatible with PointXYZIRCAEDT or PointXYZIRC. Aborting" );
558-
559- if (is_data_layout_compatible_with_point_xyziradrt (*cloud)) {
560- RCLCPP_ERROR (
561- get_logger (),
562- " The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy "
563- " code/data" );
564- }
565-
566- if (is_data_layout_compatible_with_point_xyzi (*cloud)) {
567- RCLCPP_ERROR (
568- get_logger (),
569- " The pointcloud layout is compatible with PointXYZI. You may be using legacy "
570- " code/data" );
571- }
572-
573- return false ;
574- }
575-
576- // secondly, verify the total size of the point cloud
577- if (cloud->width * cloud->height * cloud->point_step != cloud->data .size ()) {
578- RCLCPP_WARN (
579- this ->get_logger (),
580- " Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, "
581- " and frame %s received!" ,
582- cloud->data .size (), cloud->width , cloud->height , cloud->point_step ,
583- rclcpp::Time (cloud->header .stamp ).seconds (), cloud->header .frame_id .c_str ());
584- return false ;
585- }
586- return true ;
587- }
588-
589360} // namespace autoware::crop_box_filter
590361
591362#include < rclcpp_components/register_node_macro.hpp>
0 commit comments