diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 74e4de15becf3..7cfb5a52a82dd 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -3,22 +3,15 @@ checkersReport constParameterReference constVariableReference -// cspell: ignore cstyle -cstyleCast funcArgNamesDifferent functionConst functionStatic -invalidPointerCast knownConditionTrueFalse missingInclude missingIncludeSystem noConstructor -noExplicitConstructor -noValidConfiguration passedByValue -preprocessorErrorDirective redundantInitialization -shadowFunction shadowVariable // cspell: ignore uninit uninitMemberVar diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1f25372c14d71..47ddd77bdf68e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -6,7 +6,7 @@ common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.j common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp -common/autoware_point_types/** taichi.higashide@tier4.jp +common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -25,7 +25,7 @@ common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.sa common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp -common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +common/perception_utils/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -45,7 +45,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -63,6 +63,7 @@ control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4 control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/predicted_path_checker/** berkay@leodrive.ai evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp +evaluator/autoware_evaluator_utils/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -70,9 +71,9 @@ evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4. evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +launch/tier4_perception_launch/** shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -81,57 +82,57 @@ launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_landmark_based_localizer/autoware_landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_pose_covariance_modifier/** melike@leodrive.ai -localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp -localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/autoware_map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp +perception/autoware_object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp -perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp -perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp +perception/lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com yoshi.ri@tier4.jp +perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com yoshi.ri@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/radar_object_tracker/** satoshi.tanaka@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp @@ -200,11 +201,11 @@ planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4. planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp -sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp -sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp +sensing/imu_corrector/** taiki.yamada@tier4.jp yamato.ando@tier4.jp +sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index a95a070d19bd4..287769d0708ac 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -70,6 +70,14 @@ jobs: restore-keys: | ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}- + # Limit ccache size only for CUDA builds to avoid running out of disk space + - name: Limit ccache size + if: ${{ matrix.container-suffix == '-cuda' }} + run: | + rm -f "${CCACHE_DIR}/ccache.conf" + echo -e "# Set maximum cache size\nmax_size = 1MB" >> "${CCACHE_DIR}/ccache.conf" + shell: bash + - name: Show ccache stats before build run: du -sh ${CCACHE_DIR} && ccache -s shell: bash diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml index 63f15cb9da319..dc4fed1084bc0 100644 --- a/.github/workflows/cppcheck-differential.yaml +++ b/.github/workflows/cppcheck-differential.yaml @@ -58,7 +58,7 @@ jobs: run: | filtered_paths="" for dir in ${{ steps.get-full-paths.outputs.full-paths }}; do - if [ -d "$dir" ] && find "$dir" -name "*.cpp" -o -name "*.hpp" | grep -q .; then + if [ -d "$dir" ] && find "$dir" -name "*.cpp" | grep -q .; then filtered_paths="$filtered_paths $dir" fi done diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp index 4175a04aadadc..e1220bf2c5879 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp @@ -32,7 +32,7 @@ struct VehicleKinematics using Message = autoware_adapi_v1_msgs::msg::VehicleKinematics; static constexpr char name[] = "/api/vehicle/kinematics"; static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; }; diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index da37d04550f5e..126e66772920a 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -991,34 +991,51 @@ calcCurvature>( * curvature calculation */ template -std::vector> calcCurvatureAndArcLength(const T & points) +std::vector>> calcCurvatureAndSegmentLength( + const T & points) { - // Note that arclength is for the segment, not the sum. - std::vector> curvature_arc_length_vec; - curvature_arc_length_vec.emplace_back(0.0, 0.0); + // segment length is pair of segment length between {p1, p2} and {p2, p3} + std::vector>> curvature_and_segment_length_vec; + curvature_and_segment_length_vec.reserve(points.size()); + curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0)); for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); const auto p2 = autoware::universe_utils::getPoint(points.at(i)); const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3); - const double arc_length = - autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) + - autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); - curvature_arc_length_vec.emplace_back(curvature, arc_length); + + // The first point has only the next point, so put the distance to that point. + // In other words, assign the first segment length at the second point to the + // second_segment_length at the first point. + if (i == 1) { + curvature_and_segment_length_vec.at(0).second.second = + autoware::universe_utils::calcDistance2d(p1, p2); + } + + // The second_segment_length of the previous point and the first segment length of the current + // point are equal. + const std::pair arc_length{ + curvature_and_segment_length_vec.back().second.second, + autoware::universe_utils::calcDistance2d(p2, p3)}; + + curvature_and_segment_length_vec.emplace_back(curvature, arc_length); } - curvature_arc_length_vec.emplace_back(0.0, 0.0); - return curvature_arc_length_vec; + // set to the last point + curvature_and_segment_length_vec.emplace_back( + 0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0)); + + return curvature_and_segment_length_vec; } -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); /** diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index 5d536c0772fea..ce26952c3d634 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -238,14 +238,14 @@ calcCurvature>( const std::vector & points); // -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); // diff --git a/common/autoware_point_types/include/autoware_point_types/types.hpp b/common/autoware_point_types/include/autoware_point_types/types.hpp index 71ea80c5a9733..a3b0670280530 100644 --- a/common/autoware_point_types/include/autoware_point_types/types.hpp +++ b/common/autoware_point_types/include/autoware_point_types/types.hpp @@ -54,6 +54,23 @@ enum ReturnType : uint8_t { DUAL_ONLY, }; +struct PointXYZIRC +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + + friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel; + } +}; + struct PointXYZIRADRT { float x{0.0F}; @@ -75,25 +92,97 @@ struct PointXYZIRADRT } }; -enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp }; +struct PointXYZIRCAEDT +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + float azimuth{0.0F}; + float elevation{0.0F}; + float distance{0.0F}; + std::uint32_t time_stamp{0U}; + + friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel && + float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && + p1.time_stamp == p2.time_stamp; + } +}; + +enum class PointXYZIIndex { X, Y, Z, Intensity }; +enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; +enum class PointXYZIRADRTIndex { + X, + Y, + Z, + Intensity, + Ring, + Azimuth, + Distance, + ReturnType, + TimeStamp +}; +enum class PointXYZIRCAEDTIndex { + X, + Y, + Z, + Intensity, + ReturnType, + Channel, + Azimuth, + Elevation, + Distance, + TimeStamp +}; LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); + +using PointXYZIRCGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator>; + using PointXYZIRADRTGenerator = std::tuple< point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, field_return_type_generator, field_time_stamp_generator>; +using PointXYZIRCAEDTGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator, field_azimuth_generator, + field_elevation_generator, field_distance_generator, field_time_stamp_generator>; + } // namespace autoware_point_types +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware_point_types::PointXYZIRC, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) + POINT_CLOUD_REGISTER_POINT_STRUCT( autoware_point_types::PointXYZIRADRT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( double, time_stamp, time_stamp)) +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware_point_types::PointXYZIRCAEDT, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, + return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( + float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) #endif // AUTOWARE_POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp index 27ab1ef993ab3..16887ac2aa498 100644 --- a/common/autoware_point_types/test/test_point_types.cpp +++ b/common/autoware_point_types/test/test_point_types.cpp @@ -36,6 +36,15 @@ TEST(PointEquality, PointXYZIRADRT) EXPECT_EQ(pt0, pt1); } +TEST(PointEquality, PointXYZIRCAEDT) +{ + using autoware_point_types::PointXYZIRCAEDT; + + PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + EXPECT_EQ(pt0, pt1); +} + TEST(PointEquality, FloatEq) { // test template diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index 9e86ddeb60692..2fdeef2119ab8 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -1,11 +1,15 @@ cmake_minimum_required(VERSION 3.14) project(autoware_universe_utils) +option(BUILD_EXAMPLES "Build examples" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) +find_package(fmt REQUIRED) + ament_auto_add_library(autoware_universe_utils SHARED src/geometry/geometry.cpp src/geometry/pose_deviation.cpp @@ -16,6 +20,11 @@ ament_auto_add_library(autoware_universe_utils SHARED src/ros/marker_helper.cpp src/ros/logger_level_configure.cpp src/system/backtrace.cpp + src/system/time_keeper.cpp +) + +target_link_libraries(autoware_universe_utils + fmt::fmt ) if(BUILD_TESTING) @@ -30,4 +39,20 @@ if(BUILD_TESTING) ) endif() +if(BUILD_EXAMPLES) + message(STATUS "Building examples") + file(GLOB_RECURSE example_files examples/*.cpp) + + foreach(example_file ${example_files}) + get_filename_component(example_name ${example_file} NAME_WE) + add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} + autoware_universe_utils + ) + install(TARGETS ${example_name} + DESTINATION lib/${PROJECT_NAME} + ) + endforeach() +endif() + ament_auto_package() diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 22b9355b09635..cd5e366c520fc 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -7,3 +7,175 @@ This package contains many common functions used by other packages, so please re ## For developers `autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. + +## `autoware::universe_utils` + +### `systems` + +#### `autoware::universe_utils::TimeKeeper` + +##### Constructor + +```cpp +template +explicit TimeKeeper(Reporters... reporters); +``` + +- Initializes the `TimeKeeper` with a list of reporters. + +##### Methods + +- `void add_reporter(std::ostream * os);` + + - Adds a reporter to output processing times to an `ostream`. + - `os`: Pointer to the `ostream` object. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher with `std_msgs::msg::String`. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void start_track(const std::string & func_name);` + + - Starts tracking the processing time of a function. + - `func_name`: Name of the function to be tracked. + +- `void end_track(const std::string & func_name);` + - Ends tracking the processing time of a function. + - `func_name`: Name of the function to end tracking. + +##### Note + +- It's possible to start and end time measurements using `start_track` and `end_track` as shown below: + + ```cpp + time_keeper.start_track("example_function"); + // Your function code here + time_keeper.end_track("example_function"); + ``` + +- For safety and to ensure proper tracking, it is recommended to use `ScopedTimeTrack`. + +##### Example + +```cpp +#include + +#include + +#include +#include +#include +#include + +class ExampleNode : public rclcpp::Node +{ +public: + ExampleNode() : Node("time_keeper_example") + { + publisher_ = + create_publisher("processing_time", 1); + + time_keeper_ = std::make_shared(publisher_, &std::cerr); + // You can also add a reporter later by add_reporter. + // time_keeper_->add_reporter(publisher_); + // time_keeper_->add_reporter(&std::cerr); + + timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&ExampleNode::func_a, this)); + } + +private: + std::shared_ptr time_keeper_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_str_; + rclcpp::TimerBase::SharedPtr timer_; + + void func_a() + { + // Start constructing ProcessingTimeTree (because func_a is the root function) + autoware::universe_utils::ScopedTimeTrack st("func_a", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + func_b(); + // End constructing ProcessingTimeTree. After this, the tree will be reported (publishing + // message and outputting to std::cerr) + } + + void func_b() + { + autoware::universe_utils::ScopedTimeTrack st("func_b", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + func_c(); + } + + void func_c() + { + autoware::universe_utils::ScopedTimeTrack st("func_c", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + func_a (6.382ms) + └── func_b (5.243ms) + └── func_c (3.146ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + --- + nodes: + - id: 1 + name: func_a + processing_time: 6.397 + parent_id: 0 + - id: 2 + name: func_b + processing_time: 5.263 + parent_id: 1 + - id: 3 + name: func_c + processing_time: 3.129 + parent_id: 2 + ``` + +#### `autoware::universe_utils::ScopedTimeTrack` + +##### Description + +Class for automatically tracking the processing time of a function within a scope. + +##### Constructor + +```cpp +ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); +``` + +- `func_name`: Name of the function to be tracked. +- `time_keeper`: Reference to the `TimeKeeper` object. + +##### Destructor + +```cpp +~ScopedTimeTrack(); +``` + +- Destroys the `ScopedTimeTrack` object, ending the tracking of the function. diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp new file mode 100644 index 0000000000000..c50aa1e1a8342 --- /dev/null +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -0,0 +1,78 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include + +#include +#include +#include +#include + +class ExampleNode : public rclcpp::Node +{ +public: + ExampleNode() : Node("time_keeper_example") + { + publisher_ = + create_publisher("processing_time", 1); + + time_keeper_ = std::make_shared(publisher_, &std::cerr); + // You can also add a reporter later by add_reporter. + // time_keeper_->add_reporter(publisher_); + // time_keeper_->add_reporter(&std::cerr); + + timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&ExampleNode::func_a, this)); + } + +private: + std::shared_ptr time_keeper_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_str_; + rclcpp::TimerBase::SharedPtr timer_; + + void func_a() + { + // Start constructing ProcessingTimeTree (because func_a is the root function) + autoware::universe_utils::ScopedTimeTrack st("func_a", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + func_b(); + // End constructing ProcessingTimeTree. After this, the tree will be reported (publishing + // message and outputting to std::cerr) + } + + void func_b() + { + autoware::universe_utils::ScopedTimeTrack st("func_b", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + func_c(); + } + + void func_c() + { + autoware::universe_utils::ScopedTimeTrack st("func_c", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp index 87c06dfd9cf08..e6dd57c9d3fed 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp @@ -413,7 +413,7 @@ geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform); + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform); geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform); diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp new file mode 100644 index 0000000000000..c635138416aa5 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -0,0 +1,196 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ + +#include "autoware/universe_utils/system/stop_watch.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::universe_utils +{ +/** + * @brief Class representing a node in the time tracking tree + */ +class ProcessingTimeNode : public std::enable_shared_from_this +{ +public: + /** + * @brief Construct a new ProcessingTimeNode object + * + * @param name Name of the node + */ + explicit ProcessingTimeNode(const std::string & name); + + /** + * @brief Add a child node + * + * @param name Name of the child node + * @return std::shared_ptr Shared pointer to the newly added child node + */ + std::shared_ptr add_child(const std::string & name); + + /** + * @brief Get the result string representing the node and its children in a tree structure + * + * @return std::string Result string representing the node and its children + */ + std::string to_string() const; + + /** + * @brief Construct a ProcessingTimeTree message from the node and its children + * + * @return tier4_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree message + */ + tier4_debug_msgs::msg::ProcessingTimeTree to_msg() const; + + /** + * @brief Get the parent node + * + * @return std::shared_ptr Shared pointer to the parent node + */ + std::shared_ptr get_parent_node() const; + + /** + * @brief Get the child nodes + * + * @return std::vector> Vector of shared pointers to the child + * nodes + */ + std::vector> get_child_nodes() const; + + /** + * @brief Set the processing time for the node + * + * @param processing_time Processing time to be set + */ + void set_time(const double processing_time); + + /** + * @brief Get the name of the node + * + * @return std::string Name of the node + */ + std::string get_name() const; + +private: + const std::string name_; //!< Name of the node + double processing_time_{0.0}; //!< Processing time of the node + std::shared_ptr parent_node_{nullptr}; //!< Shared pointer to the parent node + std::vector> + child_nodes_; //!< Vector of shared pointers to the child nodes +}; + +using ProcessingTimeDetail = + tier4_debug_msgs::msg::ProcessingTimeTree; //!< Alias for the ProcessingTimeTree message + +/** + * @brief Class for tracking and reporting the processing time of various functions + */ +class TimeKeeper +{ +public: + template + explicit TimeKeeper(Reporters... reporters) : current_time_node_(nullptr), root_node_(nullptr) + { + reporters_.reserve(sizeof...(Reporters)); + (add_reporter(reporters), ...); + } + + /** + * @brief Add a reporter to output processing times to an ostream + * + * @param os Pointer to the ostream object + */ + void add_reporter(std::ostream * os); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Start tracking the processing time of a function + * + * @param func_name Name of the function to be tracked + */ + void start_track(const std::string & func_name); + + /** + * @brief End tracking the processing time of a function + * + * @param func_name Name of the function to end tracking + */ + void end_track(const std::string & func_name); + +private: + /** + * @brief Report the processing times to all registered reporters + */ + void report(); + + std::shared_ptr + current_time_node_; //!< Shared pointer to the current time node + std::shared_ptr root_node_; //!< Shared pointer to the root time node + autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; //!< StopWatch object for tracking the processing time + + std::vector &)>> + reporters_; //!< Vector of functions for reporting the processing times +}; + +/** + * @brief Class for automatically tracking the processing time of a function within a scope + */ +class ScopedTimeTrack +{ +public: + /** + * @brief Construct a new ScopedTimeTrack object + * + * @param func_name Name of the function to be tracked + * @param time_keeper Reference to the TimeKeeper object + */ + ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); + + ScopedTimeTrack(const ScopedTimeTrack &) = delete; + ScopedTimeTrack & operator=(const ScopedTimeTrack &) = delete; + ScopedTimeTrack(ScopedTimeTrack &&) = delete; + ScopedTimeTrack & operator=(ScopedTimeTrack &&) = delete; + + /** + * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function + */ + ~ScopedTimeTrack(); + +private: + const std::string func_name_; //!< Name of the function being tracked + TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object +}; + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/common/autoware_universe_utils/src/geometry/geometry.cpp b/common/autoware_universe_utils/src/geometry/geometry.cpp index 1f4fd318e227b..b0f5756fc2d94 100644 --- a/common/autoware_universe_utils/src/geometry/geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/geometry.cpp @@ -225,7 +225,7 @@ geometry_msgs::msg::Pose transformPose( } geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform) + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform) { geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped.transform = transform; diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp new file mode 100644 index 0000000000000..429f063dfc62e --- /dev/null +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -0,0 +1,167 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include + +namespace autoware::universe_utils +{ + +ProcessingTimeNode::ProcessingTimeNode(const std::string & name) : name_(name) +{ +} + +std::shared_ptr ProcessingTimeNode::add_child(const std::string & name) +{ + auto new_child_node = std::make_shared(name); + new_child_node->parent_node_ = shared_from_this(); + child_nodes_.push_back(new_child_node); + return new_child_node; +} + +std::string ProcessingTimeNode::to_string() const +{ + std::function + construct_string = [&]( + const ProcessingTimeNode & node, std::ostringstream & oss, + const std::string & prefix, bool is_last, bool is_root) { + if (!is_root) { + oss << prefix << (is_last ? "└── " : "├── "); + } + oss << node.name_ << " (" << node.processing_time_ << "ms)\n"; + for (size_t i = 0; i < node.child_nodes_.size(); ++i) { + const auto & child = node.child_nodes_[i]; + construct_string( + *child, oss, prefix + (is_last ? " " : "│ "), i == node.child_nodes_.size() - 1, + false); + } + }; + + std::ostringstream oss; + construct_string(*this, oss, "", true, true); + return oss.str(); +} + +tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const +{ + tier4_debug_msgs::msg::ProcessingTimeTree time_tree_msg; + + std::function + construct_msg = [&]( + const ProcessingTimeNode & node, + tier4_debug_msgs::msg::ProcessingTimeTree & tree_msg, int parent_id) { + tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; + time_node_msg.name = node.name_; + time_node_msg.processing_time = node.processing_time_; + time_node_msg.id = static_cast(tree_msg.nodes.size() + 1); + time_node_msg.parent_id = parent_id; + tree_msg.nodes.emplace_back(time_node_msg); + + for (const auto & child : node.child_nodes_) { + construct_msg(*child, tree_msg, time_node_msg.id); + } + }; + construct_msg(*this, time_tree_msg, 0); + + return time_tree_msg; +} + +std::shared_ptr ProcessingTimeNode::get_parent_node() const +{ + return parent_node_; +} +std::vector> ProcessingTimeNode::get_child_nodes() const +{ + return child_nodes_; +} +void ProcessingTimeNode::set_time(const double processing_time) +{ + processing_time_ = processing_time; +} +std::string ProcessingTimeNode::get_name() const +{ + return name_; +} + +void TimeKeeper::add_reporter(std::ostream * os) +{ + reporters_.emplace_back([os](const std::shared_ptr & node) { + *os << "==========================" << std::endl; + *os << node->to_string() << std::endl; + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + publisher->publish(node->to_msg()); + }); +} + +void TimeKeeper::start_track(const std::string & func_name) +{ + if (current_time_node_ == nullptr) { + current_time_node_ = std::make_shared(func_name); + root_node_ = current_time_node_; + } else { + current_time_node_ = current_time_node_->add_child(func_name); + } + stop_watch_.tic(func_name); +} + +void TimeKeeper::end_track(const std::string & func_name) +{ + if (current_time_node_->get_name() != func_name) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but end_track({}) is called", + current_time_node_->get_name(), func_name)); + } + const double processing_time = stop_watch_.toc(func_name); + current_time_node_->set_time(processing_time); + current_time_node_ = current_time_node_->get_parent_node(); + + if (current_time_node_ == nullptr) { + report(); + } +} + +void TimeKeeper::report() +{ + if (current_time_node_ != nullptr) { + throw std::runtime_error(fmt::format( + "You must call end_track({}) first, but report() is called", current_time_node_->get_name())); + } + for (const auto & reporter : reporters_) { + reporter(root_node_); + } + current_time_node_.reset(); + root_node_.reset(); +} + +ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper) +: func_name_(func_name), time_keeper_(time_keeper) +{ + time_keeper_.start_track(func_name_); +} + +ScopedTimeTrack::~ScopedTimeTrack() +{ + time_keeper_.end_track(func_name_); +} + +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp new file mode 100644 index 0000000000000..71ca7cc74bec5 --- /dev/null +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -0,0 +1,53 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include +#include + +#include + +#include +#include +#include + +TEST(system, TimeKeeper) +{ + using autoware::universe_utils::ScopedTimeTrack; + using autoware::universe_utils::TimeKeeper; + + rclcpp::Node node{"sample_node"}; + + auto publisher = node.create_publisher( + "~/debug/processing_time_tree", 1); + + TimeKeeper time_keeper(&std::cerr, publisher); + + ScopedTimeTrack st{"main_func", time_keeper}; + + { // funcA + ScopedTimeTrack st{"funcA", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + { // funcB + ScopedTimeTrack st{"funcB", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + { // funcC + ScopedTimeTrack st{"funcC", time_keeper}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } +} diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index 163bd761407c5..77e9294cb48c2 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -96,9 +96,7 @@ TEST_F(FakeNodeFixture, Test) } INSTANTIATE_TEST_SUITE_P( - FakeNodeFixtureTests, FakeNodeFixtureParametrized, - // cppcheck-suppress syntaxError // cppcheck doesn't like the trailing comma. - ::testing::Values(-5, 0, 42)); + FakeNodeFixtureTests, FakeNodeFixtureParametrized, ::testing::Values(-5, 0, 42)); /// @test Test that we can use a parametrized test. TEST_P(FakeNodeFixtureParametrized, Test) diff --git a/common/object_recognition_utils/src/conversion.cpp b/common/object_recognition_utils/src/conversion.cpp index c8b8e6585f5a7..ac65b35c6809a 100644 --- a/common/object_recognition_utils/src/conversion.cpp +++ b/common/object_recognition_utils/src/conversion.cpp @@ -46,7 +46,7 @@ DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects) autoware_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header = tracked_objects.header; - for (auto & tracked_object : tracked_objects.objects) { + for (const auto & tracked_object : tracked_objects.objects) { detected_objects.objects.push_back(toDetectedObject(tracked_object)); } return detected_objects; @@ -74,7 +74,7 @@ TrackedObjects toTrackedObjects(const DetectedObjects & detected_objects) TrackedObjects tracked_objects; tracked_objects.header = detected_objects.header; - for (auto & detected_object : detected_objects.objects) { + for (const auto & detected_object : detected_objects.objects) { tracked_objects.objects.push_back(toTrackedObject(detected_object)); } return tracked_objects; diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp index caa89c79d08ea..4644ec6c3e019 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -37,7 +37,6 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestOsqpInterface, BasicQp) { using autoware::common::osqp::calCSCMatrix; diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 9d5fbf40a4e89..a6bfbc42090fe 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -4,7 +4,6 @@ perception_utils 0.1.0 The perception_utils package - Satoshi Tanaka Shunsuke Miura Yoshi Ri Apache License 2.0 diff --git a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp index 74ec4c1282f46..8ec7280cfc100 100644 --- a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp +++ b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp @@ -21,17 +21,17 @@ #include -namespace qp +namespace autoware::common { /// \brief Compressed-Column-Sparse Matrix struct CSC_Matrix { /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector m_vals; + std::vector vals_; /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector m_row_idxs; + std::vector row_idxs_; /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector m_col_idxs; + std::vector col_idxs_; }; /// \brief Calculate CSC matrix from Eigen matrix @@ -41,6 +41,6 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); /// \brief Print the given CSC matrix to the standard output void printCSCMatrix(const CSC_Matrix & csc_mat); -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp index ef2c3bd17c89e..14c03a91d8fa1 100644 --- a/common/qp_interface/include/qp_interface/osqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -24,9 +24,9 @@ #include #include -namespace qp +namespace autoware::common { -constexpr c_float INF = 1e30; +constexpr c_float OSQP_INF = 1e30; class OSQPInterface : public QPInterface { @@ -34,7 +34,9 @@ class OSQPInterface : public QPInterface /// \brief Constructor without problem formulation OSQPInterface( const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); + const c_float eps_abs = std::numeric_limits::epsilon(), + const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, + const bool verbose = false); /// \brief Constructor with problem setup /// \param P: (n,n) matrix defining relations between parameters. /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. @@ -60,8 +62,10 @@ class OSQPInterface : public QPInterface CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, const std::vector & u); - int getIteration() const override; - int getStatus() const override; + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; + int getPolishStatus() const; std::vector getDualSolution() const; @@ -96,20 +100,18 @@ class OSQPInterface : public QPInterface void updateCheckTermination(const int check_termination); /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } + inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } /// \brief Get the status message for the latest problem solved inline std::string getStatusMessage() const { - return static_cast(m_latest_work_info.status); + return static_cast(latest_work_info_.status); } /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return m_latest_work_info.run_time; } + inline double getRunTime() const { return latest_work_info_.run_time; } /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return m_latest_work_info.obj_val; } + inline double getObjVal() const { return latest_work_info_.obj_val; } /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return m_exitflag; } - - void logUnsolvedStatus(const std::string & prefix_message = "") const; + inline int64_t getExitFlag() const { return exitflag_; } // Setter functions for warm start bool setWarmStart( @@ -118,17 +120,17 @@ class OSQPInterface : public QPInterface bool setDualVariables(const std::vector & dual_variables); private: - std::unique_ptr> m_work; - std::unique_ptr m_settings; - std::unique_ptr m_data; + std::unique_ptr> work_; + std::unique_ptr settings_; + std::unique_ptr data_; // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo m_latest_work_info; + OSQPInfo latest_work_info_; // Number of parameters to optimize - int64_t m_param_n; + int64_t param_n_; // Flag to check if the current work exists - bool m_work_initialized = false; + bool work__initialized = false; // Exitflag - int64_t m_exitflag; + int64_t exitflag_; void initializeProblemImpl( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -140,6 +142,6 @@ class OSQPInterface : public QPInterface std::vector optimizeImpl() override; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/proxqp_interface.hpp b/common/qp_interface/include/qp_interface/proxqp_interface.hpp index a940262d5f6da..f75fb3da5954c 100644 --- a/common/qp_interface/include/qp_interface/proxqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/proxqp_interface.hpp @@ -22,27 +22,29 @@ #include #include +#include #include -namespace qp +namespace autoware::common { class ProxQPInterface : public QPInterface { public: explicit ProxQPInterface( - const bool enable_warm_start = false, - const double eps_abs = std::numeric_limits::epsilon()); + const bool enable_warm_start, const double eps_abs, const double eps_rel, + const bool verbose = false); - int getIteration() const override; - int getStatus() const override; + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; void updateEpsAbs(const double eps_abs) override; void updateEpsRel(const double eps_rel) override; void updateVerbose(const bool verbose) override; private: - proxsuite::proxqp::Settings m_settings; - std::shared_ptr> m_qp_ptr; + proxsuite::proxqp::Settings settings_{}; + std::shared_ptr> qp_ptr_{nullptr}; void initializeProblemImpl( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -50,6 +52,6 @@ class ProxQPInterface : public QPInterface std::vector optimizeImpl() override; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/qp_interface.hpp b/common/qp_interface/include/qp_interface/qp_interface.hpp index 026c0fe413b71..85e2d103cde7a 100644 --- a/common/qp_interface/include/qp_interface/qp_interface.hpp +++ b/common/qp_interface/include/qp_interface/qp_interface.hpp @@ -18,28 +18,30 @@ #include #include +#include #include -namespace qp +namespace autoware::common { class QPInterface { public: - explicit QPInterface(const bool enable_warm_start) : m_enable_warm_start(enable_warm_start) {} + explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} std::vector optimize( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u); - virtual int getIteration() const = 0; - virtual int getStatus() const = 0; + virtual bool isSolved() const = 0; + virtual int getIterationNumber() const = 0; + virtual std::string getStatus() const = 0; virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; virtual void updateVerbose([[maybe_unused]] const bool verbose) {} protected: - bool m_enable_warm_start; + bool enable_warm_start_{false}; void initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -51,9 +53,9 @@ class QPInterface virtual std::vector optimizeImpl() = 0; - std::optional m_variables_num; - std::optional m_constraints_num; + std::optional variables_num_{std::nullopt}; + std::optional constraints_num_{std::nullopt}; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/qp_interface/src/osqp_csc_matrix_conv.cpp b/common/qp_interface/src/osqp_csc_matrix_conv.cpp index 0faf7586463fd..77a481f442000 100644 --- a/common/qp_interface/src/osqp_csc_matrix_conv.cpp +++ b/common/qp_interface/src/osqp_csc_matrix_conv.cpp @@ -21,7 +21,7 @@ #include #include -namespace qp +namespace autoware::common { CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) { @@ -114,21 +114,21 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) void printCSCMatrix(const CSC_Matrix & csc_mat) { std::cout << "["; - for (const c_float & val : csc_mat.m_vals) { + for (const c_float & val : csc_mat.vals_) { std::cout << val << ", "; } std::cout << "]\n"; std::cout << "["; - for (const c_int & row : csc_mat.m_row_idxs) { + for (const c_int & row : csc_mat.row_idxs_) { std::cout << row << ", "; } std::cout << "]\n"; std::cout << "["; - for (const c_int & col : csc_mat.m_col_idxs) { + for (const c_int & col : csc_mat.col_idxs_) { std::cout << col << ", "; } std::cout << "]\n"; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp index 81d2ffeeaaed5..0e8cb7e13988a 100644 --- a/common/qp_interface/src/osqp_interface.cpp +++ b/common/qp_interface/src/osqp_interface.cpp @@ -18,27 +18,29 @@ #include -namespace qp -{ -OSQPInterface::OSQPInterface(const bool enable_warm_start, const c_float eps_abs, const bool polish) -: QPInterface(enable_warm_start), m_work{nullptr, OSQPWorkspaceDeleter} -{ - m_settings = std::make_unique(); - m_data = std::make_unique(); - - if (m_settings) { - osqp_set_default_settings(m_settings.get()); - m_settings->alpha = 1.6; // Change alpha parameter - m_settings->eps_rel = 1.0E-4; - m_settings->eps_abs = eps_abs; - m_settings->eps_prim_inf = 1.0E-4; - m_settings->eps_dual_inf = 1.0E-4; - m_settings->warm_start = true; - m_settings->max_iter = 4000; - m_settings->verbose = false; - m_settings->polish = polish; +namespace autoware::common +{ +OSQPInterface::OSQPInterface( + const bool enable_warm_start, const c_float eps_abs, const c_float eps_rel, const bool polish, + const bool verbose) +: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} +{ + settings_ = std::make_unique(); + data_ = std::make_unique(); + + if (settings_) { + osqp_set_default_settings(settings_.get()); + settings_->alpha = 1.6; // Change alpha parameter + settings_->eps_rel = eps_rel; + settings_->eps_abs = eps_abs; + settings_->eps_prim_inf = 1.0E-4; + settings_->eps_dual_inf = 1.0E-4; + settings_->warm_start = true; + settings_->max_iter = 4000; + settings_->verbose = verbose; + settings_->polish = polish; } - m_exitflag = 0; + exitflag_ = 0; } OSQPInterface::OSQPInterface( @@ -61,8 +63,8 @@ OSQPInterface::OSQPInterface( OSQPInterface::~OSQPInterface() { - if (m_data->P) free(m_data->P); - if (m_data->A) free(m_data->A); + if (data_->P) free(data_->P); + if (data_->A) free(data_->A); } void OSQPInterface::initializeProblemImpl( @@ -89,30 +91,30 @@ void OSQPInterface::initializeCSCProblemImpl( /********************** * OBJECTIVE FUNCTION **********************/ - m_param_n = static_cast(q.size()); - m_data->m = static_cast(l.size()); + param_n_ = static_cast(q.size()); + data_->m = static_cast(l.size()); /***************** * POPULATE DATA *****************/ - m_data->n = m_param_n; - if (m_data->P) free(m_data->P); - m_data->P = csc_matrix( - m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), - P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); - m_data->q = q_dyn; - if (m_data->A) free(m_data->A); - m_data->A = csc_matrix( - m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), - A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); - m_data->l = l_dyn; - m_data->u = u_dyn; + data_->n = param_n_; + if (data_->P) free(data_->P); + data_->P = csc_matrix( + data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), + P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); + data_->q = q_dyn; + if (data_->A) free(data_->A); + data_->A = csc_matrix( + data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), + A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); + data_->l = l_dyn; + data_->u = u_dyn; // Setup workspace OSQPWorkspace * workspace; - m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); - m_work.reset(workspace); - m_work_initialized = true; + exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); + work_.reset(workspace); + work__initialized = true; } void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept @@ -124,67 +126,67 @@ void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept void OSQPInterface::updateEpsAbs(const double eps_abs) { - m_settings->eps_abs = eps_abs; // for default setting - if (m_work_initialized) { - osqp_update_eps_abs(m_work.get(), eps_abs); // for current work + settings_->eps_abs = eps_abs; // for default setting + if (work__initialized) { + osqp_update_eps_abs(work_.get(), eps_abs); // for current work } } void OSQPInterface::updateEpsRel(const double eps_rel) { - m_settings->eps_rel = eps_rel; // for default setting - if (m_work_initialized) { - osqp_update_eps_rel(m_work.get(), eps_rel); // for current work + settings_->eps_rel = eps_rel; // for default setting + if (work__initialized) { + osqp_update_eps_rel(work_.get(), eps_rel); // for current work } } void OSQPInterface::updateMaxIter(const int max_iter) { - m_settings->max_iter = max_iter; // for default setting - if (m_work_initialized) { - osqp_update_max_iter(m_work.get(), max_iter); // for current work + settings_->max_iter = max_iter; // for default setting + if (work__initialized) { + osqp_update_max_iter(work_.get(), max_iter); // for current work } } void OSQPInterface::updateVerbose(const bool is_verbose) { - m_settings->verbose = is_verbose; // for default setting - if (m_work_initialized) { - osqp_update_verbose(m_work.get(), is_verbose); // for current work + settings_->verbose = is_verbose; // for default setting + if (work__initialized) { + osqp_update_verbose(work_.get(), is_verbose); // for current work } } void OSQPInterface::updateRhoInterval(const int rho_interval) { - m_settings->adaptive_rho_interval = rho_interval; // for default setting + settings_->adaptive_rho_interval = rho_interval; // for default setting } void OSQPInterface::updateRho(const double rho) { - m_settings->rho = rho; - if (m_work_initialized) { - osqp_update_rho(m_work.get(), rho); + settings_->rho = rho; + if (work__initialized) { + osqp_update_rho(work_.get(), rho); } } void OSQPInterface::updateAlpha(const double alpha) { - m_settings->alpha = alpha; - if (m_work_initialized) { - osqp_update_alpha(m_work.get(), alpha); + settings_->alpha = alpha; + if (work__initialized) { + osqp_update_alpha(work_.get(), alpha); } } void OSQPInterface::updateScaling(const int scaling) { - m_settings->scaling = scaling; + settings_->scaling = scaling; } void OSQPInterface::updatePolish(const bool polish) { - m_settings->polish = polish; - if (m_work_initialized) { - osqp_update_polish(m_work.get(), polish); + settings_->polish = polish; + if (work__initialized) { + osqp_update_polish(work_.get(), polish); } } @@ -195,9 +197,9 @@ void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter return; } - m_settings->polish_refine_iter = polish_refine_iter; - if (m_work_initialized) { - osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); + settings_->polish_refine_iter = polish_refine_iter; + if (work__initialized) { + osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); } } @@ -208,9 +210,9 @@ void OSQPInterface::updateCheckTermination(const int check_termination) return; } - m_settings->check_termination = check_termination; - if (m_work_initialized) { - osqp_update_check_termination(m_work.get(), check_termination); + settings_->check_termination = check_termination; + if (work__initialized) { + osqp_update_check_termination(work_.get(), check_termination); } } @@ -222,11 +224,11 @@ bool OSQPInterface::setWarmStart( bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) { - if (!m_work_initialized) { + if (!work__initialized) { return false; } - const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); + const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); if (result != 0) { std::cerr << "Failed to set primal variables for warm start" << std::endl; return false; @@ -237,11 +239,11 @@ bool OSQPInterface::setPrimalVariables(const std::vector & primal_variab bool OSQPInterface::setDualVariables(const std::vector & dual_variables) { - if (!m_work_initialized) { + if (!work__initialized) { return false; } - const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); + const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); if (result != 0) { std::cerr << "Failed to set dual variables for warm start" << std::endl; return false; @@ -250,27 +252,6 @@ bool OSQPInterface::setDualVariables(const std::vector & dual_variables) return true; } -void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const -{ - const int status = getStatus(); - if (status == 1) { - // No need to log since optimization was solved. - return; - } - - // create message - std::string output_message = ""; - if (prefix_message != "") { - output_message = prefix_message + " "; - } - - const auto status_message = getStatusMessage(); - output_message += "Optimization failed due to " + status_message; - - // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); -} - void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) { /* @@ -283,14 +264,12 @@ void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) c_int P_elem_N = P_sparse.nonZeros(); */ CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); } void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) { - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); } void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) @@ -303,36 +282,34 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) c_int A_elem_N = A_sparse.nonZeros(); */ CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); return; } void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) { - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); } void OSQPInterface::updateQ(const std::vector & q_new) { std::vector q_tmp(q_new.begin(), q_new.end()); double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work.get(), q_dyn); + osqp_update_lin_cost(work_.get(), q_dyn); } void OSQPInterface::updateL(const std::vector & l_new) { std::vector l_tmp(l_new.begin(), l_new.end()); double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work.get(), l_dyn); + osqp_update_lower_bound(work_.get(), l_dyn); } void OSQPInterface::updateU(const std::vector & u_new) { std::vector u_tmp(u_new.begin(), u_new.end()); double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work.get(), u_dyn); + osqp_update_upper_bound(work_.get(), u_dyn); } void OSQPInterface::updateBounds( @@ -342,45 +319,50 @@ void OSQPInterface::updateBounds( std::vector u_tmp(u_new.begin(), u_new.end()); double * l_dyn = l_tmp.data(); double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work.get(), l_dyn, u_dyn); + osqp_update_bounds(work_.get(), l_dyn, u_dyn); +} + +int OSQPInterface::getIterationNumber() const +{ + return work_->info->iter; } -int OSQPInterface::getIteration() const +std::string OSQPInterface::getStatus() const { - return m_work->info->iter; + return "OSQP_SOLVED"; } -int OSQPInterface::getStatus() const +bool OSQPInterface::isSolved() const { - return static_cast(m_latest_work_info.status_val); + return latest_work_info_.status_val == 1; } int OSQPInterface::getPolishStatus() const { - return static_cast(m_latest_work_info.status_polish); + return static_cast(latest_work_info_.status_polish); } std::vector OSQPInterface::getDualSolution() const { - double * sol_y = m_work->solution->y; - std::vector dual_solution(sol_y, sol_y + m_data->m); + double * sol_y = work_->solution->y; + std::vector dual_solution(sol_y, sol_y + data_->m); return dual_solution; } std::vector OSQPInterface::optimizeImpl() { - osqp_solve(m_work.get()); + osqp_solve(work_.get()); - double * sol_x = m_work->solution->x; - double * sol_y = m_work->solution->y; - std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); + double * sol_x = work_->solution->x; + double * sol_y = work_->solution->y; + std::vector sol_primal(sol_x, sol_x + param_n_); + std::vector sol_lagrange_multiplier(sol_y, sol_y + data_->m); - m_latest_work_info = *(m_work->info); + latest_work_info_ = *(work_->info); - if (!m_enable_warm_start) { - m_work.reset(); - m_work_initialized = false; + if (!enable_warm_start_) { + work_.reset(); + work__initialized = false; } return sol_primal; @@ -393,7 +375,17 @@ std::vector OSQPInterface::optimize( initializeCSCProblemImpl(P, A, q, l, u); const auto result = optimizeImpl(); + // show polish status if not successful + const int status_polish = static_cast(latest_work_info_.status_polish); + if (status_polish != 1) { + const auto msg = status_polish == 0 ? "unperformed" + : status_polish == -1 ? "unsuccessful" + : "unknown"; + std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" + << std::endl; + } + return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/proxqp_interface.cpp b/common/qp_interface/src/proxqp_interface.cpp index bf1f0229e1e21..507f3858cbf1b 100644 --- a/common/qp_interface/src/proxqp_interface.cpp +++ b/common/qp_interface/src/proxqp_interface.cpp @@ -14,12 +14,17 @@ #include "qp_interface/proxqp_interface.hpp" -namespace qp +namespace autoware::common { -ProxQPInterface::ProxQPInterface(const bool enable_warm_start, const double eps_abs) +using proxsuite::proxqp::QPSolverOutput; + +ProxQPInterface::ProxQPInterface( + const bool enable_warm_start, const double eps_abs, const double eps_rel, const bool verbose) : QPInterface(enable_warm_start) { - m_settings.eps_abs = eps_abs; + settings_.eps_abs = eps_abs; + settings_.eps_rel = eps_rel; + settings_.verbose = verbose; } void ProxQPInterface::initializeProblemImpl( @@ -31,28 +36,28 @@ void ProxQPInterface::initializeProblemImpl( const bool enable_warm_start = [&]() { if ( - !m_enable_warm_start // Warm start is designated - || !m_qp_ptr // QP pointer is initialized + !enable_warm_start_ // Warm start is designated + || !qp_ptr_ // QP pointer is initialized // The number of variables is the same as the previous one. - || !m_variables_num || - *m_variables_num != variables_num + || !variables_num_ || + *variables_num_ != variables_num // The number of constraints is the same as the previous one - || !m_constraints_num || *m_constraints_num != constraints_num) { + || !constraints_num_ || *constraints_num_ != constraints_num) { return false; } return true; }(); if (!enable_warm_start) { - m_qp_ptr = std::make_shared>( + qp_ptr_ = std::make_shared>( variables_num, 0, constraints_num); } - m_settings.initial_guess = + settings_.initial_guess = enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - m_qp_ptr->settings = m_settings; + qp_ptr_->settings = settings_; Eigen::SparseMatrix P_sparse(variables_num, constraints_num); P_sparse = P.sparseView(); @@ -69,53 +74,78 @@ void ProxQPInterface::initializeProblemImpl( Eigen::Map(u_std_vec.data(), u_std_vec.size()); if (enable_warm_start) { - m_qp_ptr->update( + qp_ptr_->update( P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); } else { - m_qp_ptr->init( + qp_ptr_->init( P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); } } void ProxQPInterface::updateEpsAbs(const double eps_abs) { - m_settings.eps_abs = eps_abs; + settings_.eps_abs = eps_abs; } void ProxQPInterface::updateEpsRel(const double eps_rel) { - m_settings.eps_rel = eps_rel; + settings_.eps_rel = eps_rel; } void ProxQPInterface::updateVerbose(const bool is_verbose) { - m_settings.verbose = is_verbose; + settings_.verbose = is_verbose; } -int ProxQPInterface::getIteration() const +bool ProxQPInterface::isSolved() const { - if (m_qp_ptr) { - return m_qp_ptr->results.info.iter; + if (qp_ptr_) { + return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; } - return 0; + return false; } -int ProxQPInterface::getStatus() const +int ProxQPInterface::getIterationNumber() const { - if (m_qp_ptr) { - return static_cast(m_qp_ptr->results.info.status); + if (qp_ptr_) { + return qp_ptr_->results.info.iter; } return 0; } +std::string ProxQPInterface::getStatus() const +{ + if (qp_ptr_) { + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { + return "PROXQP_SOLVED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { + return "PROXQP_MAX_ITER_REACHED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + return "PROXQP_PRIMAL_INFEASIBLE"; + } + // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { + // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; + // } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { + return "PROXQP_DUAL_INFEASIBLE"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { + return "PROXQP_NOT_RUN"; + } + } + return "None"; +} + std::vector ProxQPInterface::optimizeImpl() { - m_qp_ptr->solve(); + qp_ptr_->solve(); std::vector result; - for (Eigen::Index i = 0; i < m_qp_ptr->results.x.size(); ++i) { - result.push_back(m_qp_ptr->results.x[i]); + for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { + result.push_back(qp_ptr_->results.x[i]); } return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/qp_interface.cpp b/common/qp_interface/src/qp_interface.cpp index e7a69bac0c67c..afd26132d7769 100644 --- a/common/qp_interface/src/qp_interface.cpp +++ b/common/qp_interface/src/qp_interface.cpp @@ -18,7 +18,7 @@ #include #include -namespace qp +namespace autoware::common { void QPInterface::initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -54,8 +54,8 @@ void QPInterface::initializeProblem( initializeProblemImpl(P, A, q, l, u); - m_variables_num = q.size(); - m_constraints_num = l.size(); + variables_num_ = q.size(); + constraints_num_ = l.size(); } std::vector QPInterface::optimize( @@ -67,4 +67,4 @@ std::vector QPInterface::optimize( return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/test/test_csc_matrix_conv.cpp b/common/qp_interface/test/test_csc_matrix_conv.cpp index fc604d762d2c4..a6bd5e3df0be1 100644 --- a/common/qp_interface/test/test_csc_matrix_conv.cpp +++ b/common/qp_interface/test/test_csc_matrix_conv.cpp @@ -23,42 +23,42 @@ TEST(TestCscMatrixConv, Nominal) { - using qp::calCSCMatrix; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrix; + using autoware::common::CSC_Matrix; Eigen::MatrixXd rect1(1, 2); rect1 << 0.0, 1.0; const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_vals[0], 1.0); - ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); - ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); + ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); + EXPECT_EQ(rect_m1.vals_[0], 1.0); + ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); + EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); + ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 + EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[2], c_int(1)); Eigen::MatrixXd rect2(2, 4); rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_vals[0], 1.0); - EXPECT_EQ(rect_m2.m_vals[1], 6.0); - EXPECT_EQ(rect_m2.m_vals[2], 3.0); - EXPECT_EQ(rect_m2.m_vals[3], 7.0); - ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); - ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); - EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); + ASSERT_EQ(rect_m2.vals_.size(), size_t(4)); + EXPECT_EQ(rect_m2.vals_[0], 1.0); + EXPECT_EQ(rect_m2.vals_[1], 6.0); + EXPECT_EQ(rect_m2.vals_[2], 3.0); + EXPECT_EQ(rect_m2.vals_[3], 7.0); + ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); + EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); + ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 + EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); + EXPECT_EQ(rect_m2.col_idxs_[4], c_int(4)); // Example from http://netlib.org/linalg/html_templates/node92.html Eigen::MatrixXd square2(6, 6); @@ -66,59 +66,59 @@ TEST(TestCscMatrixConv, Nominal) 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); - EXPECT_EQ(square_m2.m_vals[0], 10.0); - EXPECT_EQ(square_m2.m_vals[1], 3.0); - EXPECT_EQ(square_m2.m_vals[2], 3.0); - EXPECT_EQ(square_m2.m_vals[3], 9.0); - EXPECT_EQ(square_m2.m_vals[4], 7.0); - EXPECT_EQ(square_m2.m_vals[5], 8.0); - EXPECT_EQ(square_m2.m_vals[6], 4.0); - EXPECT_EQ(square_m2.m_vals[7], 8.0); - EXPECT_EQ(square_m2.m_vals[8], 8.0); - EXPECT_EQ(square_m2.m_vals[9], 7.0); - EXPECT_EQ(square_m2.m_vals[10], 7.0); - EXPECT_EQ(square_m2.m_vals[11], 9.0); - EXPECT_EQ(square_m2.m_vals[12], -2.0); - EXPECT_EQ(square_m2.m_vals[13], 5.0); - EXPECT_EQ(square_m2.m_vals[14], 9.0); - EXPECT_EQ(square_m2.m_vals[15], 2.0); - EXPECT_EQ(square_m2.m_vals[16], 3.0); - EXPECT_EQ(square_m2.m_vals[17], 13.0); - EXPECT_EQ(square_m2.m_vals[18], -1.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); - EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); - EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); - EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); + ASSERT_EQ(square_m2.vals_.size(), size_t(19)); + EXPECT_EQ(square_m2.vals_[0], 10.0); + EXPECT_EQ(square_m2.vals_[1], 3.0); + EXPECT_EQ(square_m2.vals_[2], 3.0); + EXPECT_EQ(square_m2.vals_[3], 9.0); + EXPECT_EQ(square_m2.vals_[4], 7.0); + EXPECT_EQ(square_m2.vals_[5], 8.0); + EXPECT_EQ(square_m2.vals_[6], 4.0); + EXPECT_EQ(square_m2.vals_[7], 8.0); + EXPECT_EQ(square_m2.vals_[8], 8.0); + EXPECT_EQ(square_m2.vals_[9], 7.0); + EXPECT_EQ(square_m2.vals_[10], 7.0); + EXPECT_EQ(square_m2.vals_[11], 9.0); + EXPECT_EQ(square_m2.vals_[12], -2.0); + EXPECT_EQ(square_m2.vals_[13], 5.0); + EXPECT_EQ(square_m2.vals_[14], 9.0); + EXPECT_EQ(square_m2.vals_[15], 2.0); + EXPECT_EQ(square_m2.vals_[16], 3.0); + EXPECT_EQ(square_m2.vals_[17], 13.0); + EXPECT_EQ(square_m2.vals_[18], -1.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); + EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); + EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); + EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); } TEST(TestCscMatrixConv, Trapezoidal) { - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd square2(3, 3); @@ -129,33 +129,33 @@ TEST(TestCscMatrixConv, Trapezoidal) const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m1.m_vals[0], 1.0); - EXPECT_EQ(square_m1.m_vals[1], 2.0); - EXPECT_EQ(square_m1.m_vals[2], 4.0); - ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); - EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); + ASSERT_EQ(square_m1.vals_.size(), size_t(3)); + EXPECT_EQ(square_m1.vals_[0], 1.0); + EXPECT_EQ(square_m1.vals_[1], 2.0); + EXPECT_EQ(square_m1.vals_[2], 4.0); + ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); + EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m2.m_vals[0], 2.0); - EXPECT_EQ(square_m2.m_vals[1], 5.0); - EXPECT_EQ(square_m2.m_vals[2], 6.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); + ASSERT_EQ(square_m2.vals_.size(), size_t(3)); + EXPECT_EQ(square_m2.vals_[0], 2.0); + EXPECT_EQ(square_m2.vals_[1], 5.0); + EXPECT_EQ(square_m2.vals_[2], 6.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(3)); try { const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); @@ -166,10 +166,10 @@ TEST(TestCscMatrixConv, Trapezoidal) } TEST(TestCscMatrixConv, Print) { - using qp::calCSCMatrix; - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; - using qp::printCSCMatrix; + using autoware::common::calCSCMatrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; + using autoware::common::printCSCMatrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd rect1(1, 2); square1 << 1.0, 2.0, 2.0, 4.0; diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp index e5d7041469289..9a02dd4b00934 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -37,47 +37,46 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestOsqpInterface, BasicQp) { - using qp::calCSCMatrix; - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; - auto check_result = []( - const auto & solution, const int solution_status, const int polish_status) { - EXPECT_EQ(solution_status, 1); - EXPECT_EQ(polish_status, 1); + auto check_result = + [](const auto & solution, const std::string & status, const int polish_status) { + EXPECT_EQ(status, "OSQP_SOLVED"); + EXPECT_EQ(polish_status, 1); - static const auto ep = 1.0e-8; + static const auto ep = 1.0e-8; - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, ep); + }; const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -qp::INF}; - const std::vector u = {1.0, 0.7, 0.7, qp::INF}; + const std::vector l = {1.0, 0.0, 0.0, -autoware::common::OSQP_INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::common::OSQP_INF}; { // Define problem during optimization - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { // Define problem during initialization - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { @@ -88,7 +87,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); } @@ -96,12 +95,12 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { @@ -112,7 +111,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -120,9 +119,9 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix A_csc = calCSCMatrix(A); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } // add warm startup @@ -133,7 +132,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(true, 1e-6); // enable warm start + autoware::common::OSQPInterface osqp(true, 1e-6); // enable warm start osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -141,9 +140,9 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix A_csc = calCSCMatrix(A); { const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); osqp.updateCheckTermination(1); const auto primal_val = solution; @@ -156,9 +155,9 @@ TEST(TestOsqpInterface, BasicQp) { const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } // NOTE: This should be true, but currently optimize function reset the workspace, which diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp index 96466665d5172..a8b1bb29dafa0 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -38,10 +38,11 @@ namespace // y = [-2.9, 0.0, 0.2, 0.0]` // obj = 1.88 -// cppcheck-suppress syntaxError TEST(TestProxqpInterface, BasicQp) { - auto check_result = [](const auto & solution) { + auto check_result = [](const auto & solution, const std::string & status) { + EXPECT_EQ(status, "PROXQP_SOLVED"); + static const auto ep = 1.0e-8; ASSERT_EQ(solution.size(), size_t(2)); EXPECT_NEAR(solution[0], 0.3, ep); @@ -56,23 +57,26 @@ TEST(TestProxqpInterface, BasicQp) { // Define problem during optimization - qp::ProxQPInterface proxqp(false, 1e-9); + autoware::common::ProxQPInterface proxqp(false, 1e-9, 1e-9, false); const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); + const auto status = proxqp.getStatus(); + check_result(solution, status); } { // Define problem during optimization with warm start - qp::ProxQPInterface proxqp(true, 1e-9); + autoware::common::ProxQPInterface proxqp(true, 1e-9, 1e-9, false); { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); - EXPECT_NE(proxqp.getIteration(), 1); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_NE(proxqp.getIterationNumber(), 1); } { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); - EXPECT_EQ(proxqp.getIteration(), 0); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_EQ(proxqp.getIterationNumber(), 0); } } } diff --git a/common/tensorrt_common/include/tensorrt_common/simple_profiler.hpp b/common/tensorrt_common/include/tensorrt_common/simple_profiler.hpp index 31af5fc9657d9..b89950b86ee19 100644 --- a/common/tensorrt_common/include/tensorrt_common/simple_profiler.hpp +++ b/common/tensorrt_common/include/tensorrt_common/simple_profiler.hpp @@ -58,7 +58,7 @@ class SimpleProfiler : public nvinfer1::IProfiler void setProfDict(nvinfer1::ILayer * layer) noexcept; - friend std::ostream & operator<<(std::ostream & out, SimpleProfiler & value); + friend std::ostream & operator<<(std::ostream & out, const SimpleProfiler & value); private: std::string m_name; diff --git a/common/tensorrt_common/src/simple_profiler.cpp b/common/tensorrt_common/src/simple_profiler.cpp index 00aa3220b7714..fbad1b6a40cc3 100644 --- a/common/tensorrt_common/src/simple_profiler.cpp +++ b/common/tensorrt_common/src/simple_profiler.cpp @@ -78,7 +78,7 @@ void SimpleProfiler::setProfDict(nvinfer1::ILayer * layer) noexcept } } -std::ostream & operator<<(std::ostream & out, SimpleProfiler & value) +std::ostream & operator<<(std::ostream & out, const SimpleProfiler & value) { out << "========== " << value.m_name << " profile ==========" << std::endl; float totalTime = 0; diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index 2b218cd3e49f2..a22372f74a286 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -427,18 +427,18 @@ bool TrtCommon::buildEngineFromOnnx( layer->setPrecision(nvinfer1::DataType::kHALF); std::cout << "Set kHALF in " << name << std::endl; } - for (int i = num - 1; i >= 0; i--) { - nvinfer1::ILayer * layer = network->getLayer(i); - auto layer_type = layer->getType(); - std::string name = layer->getName(); - if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; + for (int j = num - 1; j >= 0; j--) { + nvinfer1::ILayer * inner_layer = network->getLayer(j); + auto inner_layer_type = inner_layer->getType(); + std::string inner_name = inner_layer->getName(); + if (inner_layer_type == nvinfer1::LayerType::kCONVOLUTION) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + std::cout << "Set kHALF in " << inner_name << std::endl; break; } - if (layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; + if (inner_layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + std::cout << "Set kHALF in " << inner_name << std::endl; break; } } diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index f040235f3f052..db27ca1b4f699 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -192,6 +192,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | Name | Unit | Type | Description | Default value | | :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | | publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | | use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | | use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 482ddc50766f8..25db9448f758b 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -14,6 +14,7 @@ # Debug publish_debug_pointcloud: false + publish_debug_markers: true # Point cloud partitioning detection_range_min_height: 0.0 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index f6df46440d841..9fd6d0192439c 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -329,6 +329,7 @@ class AEB : public rclcpp::Node // member variables bool publish_debug_pointcloud_; + bool publish_debug_markers_; bool use_predicted_trajectory_; bool use_imu_path_; bool use_pointcloud_data_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 4a6bc0ce2c11f..d2ec1763ca4dd 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -33,8 +33,6 @@ #include #endif -#include - namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index c822adb510805..0c64b9498486a 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -8,6 +8,7 @@ Tomoya Kimura Mamoru Sobue Daniel Sanchez + Kyoichi Sugahara Apache License 2.0 diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index fb0fc2d043cc5..69bedd02d201b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -41,7 +42,9 @@ #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -131,6 +134,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) } // parameter publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); + publish_debug_markers_ = declare_parameter("publish_debug_markers"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); @@ -182,6 +186,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( { using autoware::universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + updateParam(parameters, "publish_debug_markers", publish_debug_markers_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); @@ -378,7 +383,9 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) stat.addf("RSS", "%.2f", data.rss); stat.addf("Distance", "%.2f", data.distance_to_object); stat.addf("Object Speed", "%.2f", data.velocity); - addCollisionMarker(data, debug_markers); + if (publish_debug_markers_) { + addCollisionMarker(data, debug_markers); + } } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -455,7 +462,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }); // Add debug markers - { + if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, @@ -896,6 +903,33 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); point_marker.pose.position = data.position; debug_markers.markers.push_back(point_marker); + + const auto ego_map_pose = std::invoke([this]() -> std::optional { + geometry_msgs::msg::TransformStamped tf_current_pose; + geometry_msgs::msg::Pose p; + try { + tf_current_pose = tf_buffer_.lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return std::nullopt; + } + + p.orientation = tf_current_pose.transform.rotation; + p.position.x = tf_current_pose.transform.translation.x; + p.position.y = tf_current_pose.transform.translation.y; + p.position.z = tf_current_pose.transform.translation.z; + return std::make_optional(p); + }); + + if (ego_map_pose.has_value()) { + const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m; + const auto ego_front_pose = autoware::universe_utils::calcOffsetPose( + ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0); + const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( + ego_front_pose, "autonomous_emergency_braking", this->now(), 0); + autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers); + } } } // namespace autoware::motion::control::autonomous_emergency_braking diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 902790f260e5e..058eb45bfaaff 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -271,7 +271,7 @@ class MPC */ std::pair executeOptimization( const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt, - const MPCTrajectory & trajectory); + const MPCTrajectory & trajectory, const double current_velocity); /** * @brief Resample the trajectory with the MPC resampling time. @@ -386,7 +386,8 @@ class MPC * @param reference_trajectory The reference trajectory. * @param current_velocity current velocity of ego. */ - VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const; + VectorXd calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const; //!< @brief logging with warn and return false template diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index ea97e9e6d5f39..dfc4d82541bf8 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -76,8 +76,9 @@ bool MPC::calculateMPC( const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt); // solve Optimization problem - const auto [success_opt, Uex] = - executeOptimization(mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory); + const auto [success_opt, Uex] = executeOptimization( + mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory, + current_kinematics.twist.twist.linear.x); if (!success_opt) { return fail_warn_throttle("optimization failed. Stop MPC."); } @@ -543,7 +544,8 @@ MPCMatrix MPC::generateMPCMatrix( * [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U) */ std::pair MPC::executeOptimization( - const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj) + const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj, + const double current_velocity) { VectorXd Uex; @@ -576,7 +578,7 @@ std::pair MPC::executeOptimization( VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle // steering angle rate limit - VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj); + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); VectorXd ubA = steer_rate_limits * prediction_dt; VectorXd lbA = -steer_rate_limits * prediction_dt; ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; @@ -728,7 +730,8 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } -VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const +VectorXd MPC::calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const { const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { std::vector reference, limits; @@ -762,6 +765,13 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) c return reference.back(); }; + // When the vehicle is stopped, a large steer rate limit is used for the dry steering. + constexpr double steer_rate_lim = 100.0; + const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; + if (is_vehicle_stopped) { + return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon); + } + // calculate steering rate limit VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon); for (int i = 0; i < m_param.prediction_horizon; ++i) { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 5805ef7db9f86..0fd85b5822924 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -234,6 +234,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // debug values DebugValues m_debug_values; + std::optional m_prev_keep_stopped_condition{std::nullopt}; + std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 93496c85c741b..a0118092413a9 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -599,21 +599,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity static constexpr double vel_epsilon = 1e-3; - // Let vehicle start after the steering is converged for control accuracy - const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon && - m_enable_keep_stopped_until_steer_convergence && - !lateral_sync_data_.is_steer_converged; - if (keep_stopped_condition) { - auto marker = createDefaultMarker( - "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.pose = autoware::universe_utils::calcOffsetPose( - m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, - m_vehicle_width / 2 + 2.0, 1.5); - marker.text = "steering not\nconverged"; - m_pub_stop_reason_marker->publish(marker); - } - const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && @@ -672,15 +657,18 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_under_control_starting_time = is_under_control ? std::make_shared(clock_->now()) : nullptr; } + + if (m_control_state != ControlState::STOPPED) { + m_prev_keep_stopped_condition = std::nullopt; + } + // transit state // in DRIVE state if (m_control_state == ControlState::DRIVE) { if (emergency_condition) { return changeState(ControlState::EMERGENCY); } - if (!is_under_control && stopped_condition && keep_stopped_condition) { - // NOTE: When the ego is stopped on manual driving, since the driving state may transit to - // autonomous, keep_stopped_condition should be checked. + if (!is_under_control && stopped_condition) { return changeState(ControlState::STOPPED); } @@ -723,19 +711,42 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in STOPPED state if (m_control_state == ControlState::STOPPED) { - // -- debug print -- + // debug print if (has_nonzero_target_vel && !departure_condition_from_stopped) { debug_msg_once("target speed > 0, but departure condition is not met. Keep STOPPED."); } - if (has_nonzero_target_vel && keep_stopped_condition) { - debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); - } - // --------------- - if (keep_stopped_condition) { - return changeState(ControlState::STOPPED); - } if (departure_condition_from_stopped) { + // Let vehicle start after the steering is converged for dry steering + const bool current_keep_stopped_condition = + std::fabs(current_vel) < vel_epsilon && !lateral_sync_data_.is_steer_converged; + // NOTE: Dry steering is considered unnecessary when the steering is converged twice in a + // row. This is because lateral_sync_data_.is_steer_converged is not the current but + // the previous value due to the order controllers' run and sync functions. + const bool keep_stopped_condition = + !m_prev_keep_stopped_condition || + (current_keep_stopped_condition || *m_prev_keep_stopped_condition); + m_prev_keep_stopped_condition = current_keep_stopped_condition; + if (m_enable_keep_stopped_until_steer_convergence && keep_stopped_condition) { + // debug print + if (has_nonzero_target_vel) { + debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); + } + + // publish debug marker + auto marker = createDefaultMarker( + "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose = autoware::universe_utils::calcOffsetPose( + m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, + m_vehicle_width / 2 + 2.0, 1.5); + marker.text = "steering not\nconverged"; + m_pub_stop_reason_marker->publish(marker); + + // keep STOPPED + return; + } + m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); // prevent the car from taking a long time to start to move diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp index 62f5f7a5737c9..3a79b1f01a804 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp @@ -229,8 +229,8 @@ bool SplineInterpolate::interpolate( generateSpline(base_value); // interpolate by spline with normalized index - for (int i = 0; i < static_cast(normalized_idx.size()); ++i) { - return_value.push_back(getValue(normalized_idx[i])); + for (const auto & index : normalized_idx) { + return_value.push_back(getValue(index)); } return true; } diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp index e0dc5ede906a9..48ffab52e2b15 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -589,11 +589,28 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); - tester.traj_pub->publish(traj); - test_utils::waitForMessage(tester.node, this, tester.received_control_command); + { // Check if the ego can keep stopped when the steering is not converged. + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); - ASSERT_TRUE(tester.received_control_command); - // Keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + ASSERT_TRUE(tester.received_control_command); + // Keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + } + + { // Check if the ego can keep stopped after the following sequence + // 1. not converged -> 2. converged -> 3. not converged + tester.publish_steer_angle(0.0); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + tester.publish_steer_angle(steering_tire_angle); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + // Keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + } } diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 6f5c0e5cdb883..77d6df2021c8f 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -65,7 +65,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, - const double stop_margin, autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + const double stop_margin, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); bool isInBrakeDistance( const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index d750f3aa07248..48640b21b6064 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -148,7 +148,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, - autoware::vehicle_info_utils::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // It returns the stop point and segment of the point on trajectory. diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index da60c820c45b1..614f1d66b9e0d 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -52,13 +52,6 @@ class ControlEvaluatorNode : public rclcpp::Node { public: explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); - void removeOldDiagnostics(const rclcpp::Time & stamp); - void removeDiagnosticsByName(const std::string & name); - void addDiagnostic(const DiagnosticStatus & diag, const rclcpp::Time & stamp); - void updateDiagnosticQueue( - const DiagnosticArray & input_diagnostics, const std::string & function, - const rclcpp::Time & stamp); - DiagnosticStatus generateLateralDeviationDiagnosticStatus( const Trajectory & traj, const Point & ego_point); DiagnosticStatus generateYawDeviationDiagnosticStatus( diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 21f48b1c64485..d7300e6a3bfb4 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_evaluator_utils autoware_motion_utils autoware_planning_msgs autoware_route_handler diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 5862522d582f7..47706ed56cd7a 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -14,6 +14,8 @@ #include "autoware/control_evaluator/control_evaluator_node.hpp" +#include "autoware/evaluator_utils/evaluator_utils.hpp" + #include #include @@ -64,57 +66,11 @@ void ControlEvaluatorNode::getRouteData() } } -void ControlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) -{ - constexpr double KEEP_TIME = 1.0; - diag_queue_.erase( - std::remove_if( - diag_queue_.begin(), diag_queue_.end(), - [stamp](const std::pair & p) { - return (stamp - p.second).seconds() > KEEP_TIME; - }), - diag_queue_.end()); -} - -void ControlEvaluatorNode::removeDiagnosticsByName(const std::string & name) -{ - diag_queue_.erase( - std::remove_if( - diag_queue_.begin(), diag_queue_.end(), - [&name](const std::pair & p) { - return p.first.name.find(name) != std::string::npos; - }), - diag_queue_.end()); -} - -void ControlEvaluatorNode::addDiagnostic( - const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp) -{ - diag_queue_.push_back(std::make_pair(diag, stamp)); -} - -void ControlEvaluatorNode::updateDiagnosticQueue( - const DiagnosticArray & input_diagnostics, const std::string & function, - const rclcpp::Time & stamp) -{ - const auto it = std::find_if( - input_diagnostics.status.begin(), input_diagnostics.status.end(), - [&function](const diagnostic_msgs::msg::DiagnosticStatus & diag) { - return diag.name.find(function) != std::string::npos; - }); - if (it != input_diagnostics.status.end()) { - removeDiagnosticsByName(it->name); - addDiagnostic(*it, input_diagnostics.header.stamp); - } - - removeOldDiagnostics(stamp); -} - void ControlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) { // add target diagnostics to the queue and remove old ones for (const auto & function : target_functions_) { - updateDiagnosticQueue(*diag_msg, function, now()); + autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); } } diff --git a/evaluator/autoware_evaluator_utils/CMakeLists.txt b/evaluator/autoware_evaluator_utils/CMakeLists.txt new file mode 100644 index 0000000000000..d75ed43a4e3cb --- /dev/null +++ b/evaluator/autoware_evaluator_utils/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_evaluator_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(evaluator_utils SHARED + src/evaluator_utils.cpp +) + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/evaluator/autoware_evaluator_utils/README.md b/evaluator/autoware_evaluator_utils/README.md new file mode 100644 index 0000000000000..b0db86f86b5c0 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/README.md @@ -0,0 +1,5 @@ +# Evaluator Utils + +## Purpose + +This package provides utils functions for other evaluator packages diff --git a/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp b/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp new file mode 100644 index 0000000000000..3b91c9d7605e0 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ +#define AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::evaluator_utils +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using DiagnosticQueue = std::deque>; + +void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); +void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue); +void addDiagnostic( + const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); +void updateDiagnosticQueue( + const DiagnosticArray & input_diagnostics, const std::string & function, + const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); + +} // namespace autoware::evaluator_utils + +#endif // AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ diff --git a/evaluator/autoware_evaluator_utils/package.xml b/evaluator/autoware_evaluator_utils/package.xml new file mode 100644 index 0000000000000..b8566e33a32a3 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/package.xml @@ -0,0 +1,24 @@ + + + + autoware_evaluator_utils + 0.1.0 + ROS 2 node for evaluating control + Daniel SANCHEZ + Takayuki MUROOKA + Apache License 2.0 + + Daniel SANCHEZ + Takayuki MUROOKA + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + rclcpp + rclcpp_components + + + ament_cmake + + diff --git a/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp b/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp new file mode 100644 index 0000000000000..293db21f50e7f --- /dev/null +++ b/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp @@ -0,0 +1,66 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/evaluator_utils/evaluator_utils.hpp" + +#include + +namespace autoware::evaluator_utils +{ +void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + constexpr double KEEP_TIME = 1.0; + diag_queue.erase( + std::remove_if( + diag_queue.begin(), diag_queue.end(), + [stamp](const std::pair & p) { + return (stamp - p.second).seconds() > KEEP_TIME; + }), + diag_queue.end()); +} + +void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue) +{ + diag_queue.erase( + std::remove_if( + diag_queue.begin(), diag_queue.end(), + [&name](const std::pair & p) { + return p.first.name.find(name) != std::string::npos; + }), + diag_queue.end()); +} + +void addDiagnostic( + const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + diag_queue.push_back(std::make_pair(diag, stamp)); +} + +void updateDiagnosticQueue( + const DiagnosticArray & input_diagnostics, const std::string & function, + const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + const auto it = std::find_if( + input_diagnostics.status.begin(), input_diagnostics.status.end(), + [&function](const DiagnosticStatus & diag) { + return diag.name.find(function) != std::string::npos; + }); + if (it != input_diagnostics.status.end()) { + removeDiagnosticsByName(it->name, diag_queue); + addDiagnostic(*it, input_diagnostics.header.stamp, diag_queue); + } + + removeOldDiagnostics(stamp, diag_queue); +} +} // namespace autoware::evaluator_utils diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 74523398c3a77..e15427c59ce06 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -32,13 +32,14 @@ #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include +#include #include #include #include #include +#include #include - namespace planning_diagnostics { using autoware_perception_msgs::msg::PredictedObjects; @@ -70,7 +71,8 @@ class PlanningEvaluatorNode : public rclcpp::Node * @brief callback on receiving a trajectory * @param [in] traj_msg received trajectory message */ - void onTrajectory(const Trajectory::ConstSharedPtr traj_msg); + void onTrajectory( + const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief callback on receiving a reference trajectory @@ -88,7 +90,14 @@ class PlanningEvaluatorNode : public rclcpp::Node * @brief callback on receiving a modified goal * @param [in] modified_goal_msg received modified goal message */ - void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg); + void onModifiedGoal( + const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, + const Odometry::ConstSharedPtr ego_state_ptr); + + /** + * @brief obtain diagnostics information + */ + void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); /** * @brief publish the given metric statistic @@ -99,26 +108,52 @@ class PlanningEvaluatorNode : public rclcpp::Node /** * @brief publish current ego lane info */ - DiagnosticStatus generateLaneletDiagnosticStatus(); + DiagnosticStatus generateDiagnosticEvaluationStatus(const DiagnosticStatus & diag); + + /** + * @brief publish current ego lane info + */ + DiagnosticStatus generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr); /** * @brief publish current ego kinematic state */ DiagnosticStatus generateKinematicStateDiagnosticStatus( - const AccelWithCovarianceStamped & accel_stamped); + const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr); private: static bool isFinite(const TrajectoryPoint & p); - void publishModifiedGoalDeviationMetrics(); - // update Route Handler + + /** + * @brief update route handler data + */ void getRouteData(); + /** + * @brief fetch data and publish diagnostics + */ + void onTimer(); + + /** + * @brief fetch topic data + */ + void fetchData(); + // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately. + // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with + // onDiagnostics(). + rclcpp::Subscription::SharedPtr planning_diag_sub_; + // ROS - rclcpp::Subscription::SharedPtr traj_sub_; - rclcpp::Subscription::SharedPtr ref_sub_; - rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Subscription::SharedPtr modified_goal_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ + this, "~/input/trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber ref_sub_{ + this, "~/input/reference_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ + this, "~/input/objects"}; + autoware::universe_utils::InterProcessPollingSubscriber modified_goal_sub_{ + this, "~/input/modified_goal"}; + autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ + this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ this, "~/input/route", rclcpp::QoS{1}.transient_local()}; autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ @@ -131,6 +166,7 @@ class PlanningEvaluatorNode : public rclcpp::Node std::unique_ptr tf_buffer_; autoware::route_handler::RouteHandler route_handler_; + DiagnosticArray metrics_msg_; // Parameters std::string output_file_str_; std::string ego_frame_str_; @@ -142,8 +178,12 @@ class PlanningEvaluatorNode : public rclcpp::Node std::deque stamps_; std::array>, static_cast(Metric::SIZE)> metric_stats_; - Odometry::ConstSharedPtr ego_state_ptr_; - PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_; + rclcpp::TimerBase::SharedPtr timer_; + // queue for diagnostics and time stamp + std::deque> diag_queue_; + const std::vector target_functions_ = { + "obstacle_cruise_planner_stop", "obstacle_cruise_planner_slow_down", + "obstacle_cruise_planner_cruise"}; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 70301ebe6275f..c9517404b63a9 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -7,6 +7,7 @@ + @@ -15,6 +16,7 @@ + diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 3c3d4bd8bb595..eb3161e0afb35 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + autoware_evaluator_utils autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 5e959aed123af..e02367407f4ae 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -14,6 +14,8 @@ #include "autoware/planning_evaluator/planning_evaluator_node.hpp" +#include "autoware/evaluator_utils/evaluator_utils.hpp" + #include #include @@ -21,6 +23,7 @@ #include "boost/lexical_cast.hpp" +#include #include #include #include @@ -35,26 +38,13 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op : Node("planning_evaluator", node_options) { using std::placeholders::_1; - - traj_sub_ = create_subscription( - "~/input/trajectory", 1, std::bind(&PlanningEvaluatorNode::onTrajectory, this, _1)); - - ref_sub_ = create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&PlanningEvaluatorNode::onReferenceTrajectory, this, _1)); - - objects_sub_ = create_subscription( - "~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1)); - - modified_goal_sub_ = create_subscription( - "~/input/modified_goal", 1, std::bind(&PlanningEvaluatorNode::onModifiedGoal, this, _1)); - - odom_sub_ = create_subscription( - "~/input/odometry", 1, std::bind(&PlanningEvaluatorNode::onOdometry, this, _1)); - tf_buffer_ = std::make_unique(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); + // Timer callback to publish evaluator diagnostics + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&PlanningEvaluatorNode::onTimer, this)); // Parameters metrics_calculator_.parameters.trajectory.min_point_dist_m = declare_parameter("trajectory.min_point_dist_m"); @@ -68,6 +58,9 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op output_file_str_ = declare_parameter("output_file"); ego_frame_str_ = declare_parameter("ego_frame"); + planning_diag_sub_ = create_subscription( + "~/input/diagnostics", 1, std::bind(&PlanningEvaluatorNode::onDiagnostics, this, _1)); + // List of metrics to calculate and publish metrics_pub_ = create_publisher("~/metrics", 1); for (const std::string & selected_metric : @@ -110,6 +103,43 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode() } } +void PlanningEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) +{ + // add target diagnostics to the queue and remove old ones + for (const auto & function : target_functions_) { + autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); + } +} + +DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticEvaluationStatus( + const DiagnosticStatus & diag) +{ + DiagnosticStatus status; + status.name = diag.name; + + const auto it = std::find_if(diag.values.begin(), diag.values.end(), [](const auto & key_value) { + return key_value.key.find("decision") != std::string::npos; + }); + const bool found = it != diag.values.end(); + status.level = (found) ? status.OK : status.ERROR; + + auto get_key_value = [&]() { + if (!found) { + return diagnostic_msgs::msg::KeyValue{}; + } + if (it->value.find("none") != std::string::npos) { + return *it; + } + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "decision"; + key_value.value = "deceleration"; + return key_value; + }; + + status.values.push_back(get_key_value()); + return status; +} + void PlanningEvaluatorNode::getRouteData() { // route @@ -133,9 +163,10 @@ void PlanningEvaluatorNode::getRouteData() } } -DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus() +DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus( + const Odometry::ConstSharedPtr ego_state_ptr) { - const auto & ego_pose = ego_state_ptr_->pose.pose; + const auto & ego_pose = ego_state_ptr->pose.pose; const auto current_lanelets = [&]() { lanelet::ConstLanelet closest_route_lanelet; route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); @@ -166,14 +197,14 @@ DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus() } DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus( - const AccelWithCovarianceStamped & accel_stamped) + const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr) { DiagnosticStatus status; status.name = "kinematic_state"; status.level = status.OK; diagnostic_msgs::msg::KeyValue key_value; key_value.key = "vel"; - key_value.value = std::to_string(ego_state_ptr_->twist.twist.linear.x); + key_value.value = std::to_string(ego_state_ptr->twist.twist.linear.x); status.values.push_back(key_value); key_value.key = "acc"; const auto & acc = accel_stamped.accel.accel.linear.x; @@ -220,9 +251,57 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( return status; } -void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg) +void PlanningEvaluatorNode::onTimer() +{ + metrics_msg_.header.stamp = now(); + + const auto ego_state_ptr = odometry_sub_.takeData(); + onOdometry(ego_state_ptr); + { + const auto objects_msg = objects_sub_.takeData(); + onObjects(objects_msg); + } + + { + const auto ref_traj_msg = ref_sub_.takeData(); + onReferenceTrajectory(ref_traj_msg); + } + + { + const auto traj_msg = traj_sub_.takeData(); + onTrajectory(traj_msg, ego_state_ptr); + } + { + const auto modified_goal_msg = modified_goal_sub_.takeData(); + onModifiedGoal(modified_goal_msg, ego_state_ptr); + } + + { + // generate decision diagnostics from input diagnostics + for (const auto & function : target_functions_) { + const auto it = std::find_if( + diag_queue_.begin(), diag_queue_.end(), + [&function](const std::pair & p) { + return p.first.name.find(function) != std::string::npos; + }); + if (it == diag_queue_.end()) { + continue; + } + // generate each decision diagnostics + metrics_msg_.status.push_back(generateDiagnosticEvaluationStatus(it->first)); + } + } + + if (!metrics_msg_.status.empty()) { + metrics_pub_->publish(metrics_msg_); + } + metrics_msg_ = DiagnosticArray{}; +} + +void PlanningEvaluatorNode::onTrajectory( + const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr) { - if (!ego_state_ptr_) { + if (!ego_state_ptr || !traj_msg) { return; } @@ -231,8 +310,6 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m stamps_.push_back(traj_msg->header.stamp); } - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); for (Metric metric : metrics_) { const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); if (!metric_stat) { @@ -244,88 +321,71 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m } if (metric_stat->count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); } } - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } + metrics_calculator_.setPreviousTrajectory(*traj_msg); auto runtime = (now() - start).seconds(); RCLCPP_DEBUG(get_logger(), "Planning evaluation calculation time: %2.2f ms", runtime * 1e3); } void PlanningEvaluatorNode::onModifiedGoal( - const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg) + const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, + const Odometry::ConstSharedPtr ego_state_ptr) { - modified_goal_ptr_ = modified_goal_msg; - if (ego_state_ptr_) { - publishModifiedGoalDeviationMetrics(); - } -} - -void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) -{ - ego_state_ptr_ = odometry_msg; - metrics_calculator_.setEgoPose(odometry_msg->pose.pose); - { - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); - - getRouteData(); - if (route_handler_.isHandlerReady() && ego_state_ptr_) { - metrics_msg.status.push_back(generateLaneletDiagnosticStatus()); - } - - const auto acc = accel_sub_.takeData(); - - if (acc && ego_state_ptr_) { - metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*acc)); - } - - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } - } - - if (modified_goal_ptr_) { - publishModifiedGoalDeviationMetrics(); + if (!modified_goal_msg || !ego_state_ptr) { + return; } -} - -void PlanningEvaluatorNode::publishModifiedGoalDeviationMetrics() -{ auto start = now(); - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); for (Metric metric : metrics_) { const auto metric_stat = metrics_calculator_.calculate( - Metric(metric), modified_goal_ptr_->pose, ego_state_ptr_->pose.pose); + Metric(metric), modified_goal_msg->pose, ego_state_ptr->pose.pose); if (!metric_stat) { continue; } metric_stats_[static_cast(metric)].push_back(*metric_stat); if (metric_stat->count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); } } - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } auto runtime = (now() - start).seconds(); RCLCPP_DEBUG( get_logger(), "Planning evaluation modified goal deviation calculation time: %2.2f ms", runtime * 1e3); } +void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) +{ + if (!odometry_msg) return; + metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + { + getRouteData(); + if (route_handler_.isHandlerReady() && odometry_msg) { + metrics_msg_.status.push_back(generateLaneletDiagnosticStatus(odometry_msg)); + } + + const auto acc_msg = accel_sub_.takeData(); + if (acc_msg && odometry_msg) { + metrics_msg_.status.push_back(generateKinematicStateDiagnosticStatus(*acc_msg, odometry_msg)); + } + } +} + void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg) { + if (!traj_msg) { + return; + } metrics_calculator_.setReferenceTrajectory(*traj_msg); } void PlanningEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { + if (!objects_msg) { + return; + } metrics_calculator_.setPredictedObjects(*objects_msg); } diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index f23d9f74986e6..d8f3ef55b15dd 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -216,7 +216,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 150744bbae4a7..b6f3ad4c27eea 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -5,7 +5,6 @@ 0.1.0 The tier4_localization_launch package Yamato Ando - Koji Minoda Kento Yabuuchi Ryu Yamamoto NGUYEN Viet Anh diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 05643e354cfce..bca5a853d69f7 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -5,7 +5,6 @@ 0.1.0 The tier4_map_launch package Ryu Yamamoto - Koji Minoda Kento Yabuuchi Yamato Ando NGUYEN Viet Anh diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 373e343ad64fa..8f64b624540e7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -13,6 +13,7 @@ + @@ -163,6 +164,7 @@ + @@ -357,7 +359,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 4c288aa632182..9256e90ec3e35 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -4,6 +4,7 @@ + @@ -95,14 +96,24 @@ + + + + + + + + + + - + @@ -147,7 +158,7 @@ - + @@ -202,7 +213,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml index b85d4b02847b3..681febf1c5551 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml @@ -3,8 +3,8 @@ - - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py index 5fa71cd1aa0e9..6c0e5f20b8957 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py @@ -106,7 +106,7 @@ def create_compare_map_pipeline(self): components.append( ComposableNode( package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", + plugin="autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent", name="voxel_based_compare_map_filter", remappings=[ ("input", down_sample_topic), diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml index fe7ab70e9331e..151e3f28739d9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml @@ -5,28 +5,28 @@ - + - + - + - + @@ -39,7 +39,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml index 6ea745a8bae2c..5bd2c36955b55 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -89,7 +89,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 1975ad2d15eb9..580436411a895 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -46,7 +46,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 684ecee757aad..fd38b7bfd6ab3 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -332,7 +332,7 @@ def create_time_series_outlier_filter_components(input_topic, output_topic): components.append( ComposableNode( package="occupancy_grid_map_outlier_filter", - plugin="occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", + plugin="autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", name="occupancy_grid_based_outlier_filter", remappings=[ ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), @@ -391,7 +391,7 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con components.append( ComposableNode( package="compare_map_segmentation", - plugin="compare_map_segmentation::CompareElevationMapFilterComponent", + plugin="autoware::compare_map_segmentation::CompareElevationMapFilterComponent", name="compare_elevation_map_filter", namespace="elevation_map", remappings=[ diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 5d8d7563e7a7c..6c428a5adea0b 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -80,6 +80,7 @@ + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index ed7ff059a6e53..6cc47de76ea96 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 57d4b209efeef..db2968f735a37 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -7,18 +7,26 @@ Yukihiro Saito Shunsuke Miura Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto autoware_cmake + autoware_cluster_merger autoware_crosswalk_traffic_light_estimator + autoware_detection_by_tracker autoware_map_based_prediction - cluster_merger + autoware_object_range_splitter + autoware_object_velocity_splitter + autoware_radar_crossing_objects_noise_filter + autoware_radar_fusion_to_detected_object + autoware_radar_object_clustering + autoware_radar_object_tracker + autoware_raindrop_cluster_filter compare_map_segmentation detected_object_feature_remover detected_object_validation - detection_by_tracker elevation_map_loader euclidean_cluster ground_segmentation @@ -27,16 +35,10 @@ lidar_apollo_instance_segmentation multi_object_tracker object_merger - object_range_splitter - object_velocity_splitter occupancy_grid_map_outlier_filter pointcloud_preprocessor pointcloud_to_laserscan probabilistic_occupancy_grid_map - radar_crossing_objects_noise_filter - radar_fusion_to_detected_object - radar_object_clustering - radar_object_tracker shape_estimation topic_tools tracking_object_merger diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 3deff32d6e297..3c58c8d85b5be 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -260,7 +260,12 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index d0c34ea01b2c6..63353459fa16c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -90,6 +90,7 @@ + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 7dc33e0e82ea9..bf43fde85e0a2 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -148,7 +148,7 @@ - + diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 55c4403b65923..d11d00c43d55b 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -4,6 +4,7 @@ + @@ -64,6 +65,13 @@ + + + + + + + diff --git a/localization/autoware_stop_filter/package.xml b/localization/autoware_stop_filter/package.xml index dfdbd5f182983..d020f390e5bbf 100644 --- a/localization/autoware_stop_filter/package.xml +++ b/localization/autoware_stop_filter/package.xml @@ -4,7 +4,6 @@ autoware_stop_filter 0.0.0 The stop filter package - Koji Minoda Yamato Ando Masahiro Sakamoto Kento Yabuuchi diff --git a/localization/autoware_stop_filter/src/stop_filter.hpp b/localization/autoware_stop_filter/src/stop_filter.hpp index ce6e67f0b7265..b6d56b5f77059 100644 --- a/localization/autoware_stop_filter/src/stop_filter.hpp +++ b/localization/autoware_stop_filter/src/stop_filter.hpp @@ -39,7 +39,7 @@ namespace autoware::stop_filter class StopFilter : public rclcpp::Node { public: - explicit StopFilter(const rclcpp::NodeOptions & options); + explicit StopFilter(const rclcpp::NodeOptions & node_options); private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 9c89867025632..b53538de11f15 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -5,7 +5,6 @@ 0.1.0 The ekf_localizer package Takamasa Horibe - Koji Minoda Yamato Ando Takeshi Ishita Masahiro Sakamoto diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index 36b2aec8384ac..3ffa7cb1c7111 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -5,7 +5,6 @@ 0.1.0 The geo_pose_projector package Yamato Ando - Koji Minoda Masahiro Sakamoto Kento Yabuuchi NGUYEN Viet Anh diff --git a/localization/localization_error_monitor/README.md b/localization/localization_error_monitor/README.md index 7912ad843ef19..8dc82f09f3d07 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -16,9 +16,9 @@ The package monitors the following two values: ### Input -| Name | Type | Description | -| --------------------- | ----------------------------------------------- | ------------------- | -| `input/pose_with_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | localization result | +| Name | Type | Description | +| ------------ | ------------------------- | ------------------- | +| `input/odom` | `nav_msgs::msg::Odometry` | localization result | ### Output diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index b8a466f95dfab..02602cbebff37 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -5,7 +5,6 @@ 0.1.0 ros node for monitoring localization error Yamato Ando - Koji Minoda Masahiro Sakamoto Kento Yabuuchi NGUYEN Viet Anh diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 48ce49d2bcaa9..1881deab23d05 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -241,18 +241,26 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching ### Abstract -Calculate 2D covariance (xx, xy, yx, yy) in real time using the NDT convergence from multiple initial poses. -The arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. +Initially, the covariance of NDT scan matching is a fixed value(FIXED_VALUE mode). +So, three modes are provided for 2D covariance (xx, xy, yx, yy) estimation in real time: LAPLACE_APPROXIMATION, MULTI_NDT, and MULTI_NDT_SCORE. +LAPLACE_APPROXIMATION calculates the inverse matrix of the XY (2x2) part of the Hessian obtained by NDT scan matching and uses it as the covariance matrix. +On the other hand, MULTI_NDT, and MULTI_NDT_SCORE use NDT convergence from multiple initial poses to obtain 2D covariance. +Ideally, the arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. In this implementation, the number of initial positions is fixed to simplify the code. +To obtain the covariance, MULTI_NDT computes until convergence at each initial position, while MULTI_NDT_SCORE uses the nearest voxel transformation likelihood. The covariance can be seen as error ellipse from ndt_pose_with_covariance setting on rviz2. [original paper](https://www.fujipress.jp/jrm/rb/robot003500020435/). +drawing + Note that this function may spoil healthy system behavior if it consumes much calculation resources. ### Parameters +There are three types in the calculation of 2D covariance in real time.You can select the method by changing covariance_estimation_type. initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. +In MULTI_NDT_SCORE mode, the scale of the output 2D covariance can be adjusted according to the temperature. {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index f62329b8bdb6d..616cb108baf4a 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -107,13 +107,18 @@ # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) covariance_estimation: - enable: false + # Covariance estimation type + # 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE + covariance_estimation_type: 0 # Offset arrangement in covariance estimation [m] # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + # In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance + temperature: 0.1 + dynamic_map_loading: # Dynamic map loading distance diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index e488b49393d48..d6bd975c36bf3 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -29,6 +29,13 @@ enum class ConvergedParamType { NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 }; +enum class CovarianceEstimationType { + FIXED_VALUE = 0, + LAPLACE_APPROXIMATION = 1, + MULTI_NDT = 2, + MULTI_NDT_SCORE = 3, +}; + struct HyperParameters { struct Frame @@ -80,8 +87,10 @@ struct HyperParameters struct CovarianceEstimation { - bool enable{}; - std::vector initial_pose_offset_model{}; + CovarianceEstimationType covariance_estimation_type{}; + std::vector initial_pose_offset_model_x{}; + std::vector initial_pose_offset_model_y{}; + double temperature{}; } covariance_estimation{}; } covariance{}; @@ -148,33 +157,27 @@ struct HyperParameters for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { covariance.output_pose_covariance[i] = output_pose_covariance[i]; } - covariance.covariance_estimation.enable = - node->declare_parameter("covariance.covariance_estimation.enable"); - if (covariance.covariance_estimation.enable) { - std::vector initial_pose_offset_model_x = - node->declare_parameter>( - "covariance.covariance_estimation.initial_pose_offset_model_x"); - std::vector initial_pose_offset_model_y = - node->declare_parameter>( - "covariance.covariance_estimation.initial_pose_offset_model_y"); - - if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { - const size_t size = initial_pose_offset_model_x.size(); - covariance.covariance_estimation.initial_pose_offset_model.resize(size); - for (size_t i = 0; i < size; i++) { - covariance.covariance_estimation.initial_pose_offset_model[i].x() = - initial_pose_offset_model_x[i]; - covariance.covariance_estimation.initial_pose_offset_model[i].y() = - initial_pose_offset_model_y[i]; - } - } else { - std::stringstream message; - message << "Invalid initial pose offset model parameters." - << "Please make sure that the number of elements in " - << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same."; - throw std::runtime_error(message.str()); - } + const int64_t covariance_estimation_type_tmp = node->declare_parameter( + "covariance.covariance_estimation.covariance_estimation_type"); + covariance.covariance_estimation.covariance_estimation_type = + static_cast(covariance_estimation_type_tmp); + covariance.covariance_estimation.initial_pose_offset_model_x = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_x"); + covariance.covariance_estimation.initial_pose_offset_model_y = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_y"); + if ( + covariance.covariance_estimation.initial_pose_offset_model_x.size() != + covariance.covariance_estimation.initial_pose_offset_model_y.size()) { + std::stringstream message; + message << "Invalid initial pose offset model parameters." + << "Please make sure that the number of elements in " + << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same."; + throw std::runtime_error(message.str()); } + covariance.covariance_estimation.temperature = + node->declare_parameter("covariance.covariance_estimation.temperature"); dynamic_map_loading.update_distance = node->declare_parameter("dynamic_map_loading.update_distance"); diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index ac913b128b5f1..e09ff5c782e7d 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -52,6 +52,8 @@ class MapUpdateModule rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr, HyperParameters::DynamicMapLoading param); + bool out_of_map_range(const geometry_msgs::msg::Point & position); + private: friend class NDTScanMatcher; @@ -62,6 +64,7 @@ class MapUpdateModule [[nodiscard]] bool should_update_map( const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr); + void update_map( const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr); @@ -88,6 +91,8 @@ class MapUpdateModule // Indicate if there is a prefetch thread waiting for being collected NdtPtrType secondary_ndt_ptr_; bool need_rebuild_; + // Keep the last_update_position_ unchanged while checking map range + std::mutex last_update_position_mtx_; }; #endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 2f3eb82b1c217..119c3534cab16 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -131,7 +131,7 @@ class NDTScanMatcher : public rclcpp::Node static int count_oscillation(const std::vector & result_pose_msg_array); - std::array estimate_covariance( + Eigen::Matrix2d estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); diff --git a/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png b/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png new file mode 100644 index 0000000000000..f6e25d04c0fd6 Binary files /dev/null and b/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png differ diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index c69836d77c825..932ee55874cea 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -6,7 +6,6 @@ The ndt_scan_matcher package Yamato Ando Kento Yabuuchi - Koji Minoda Masahiro Sakamoto NGUYEN Viet Anh Taiki Yamada diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json index d978e5bcf7357..cbee3b5da72aa 100644 --- a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json +++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json @@ -5,10 +5,12 @@ "covariance_estimation": { "type": "object", "properties": { - "enable": { - "type": "boolean", - "description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).", - "default": false + "covariance_estimation_type": { + "type": "number", + "description": "2D Real-time Converged estimation type. 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE (FIXED_VALUE mode does not perform 2D Real-time Converged estimation)", + "default": 0, + "minimum": 0, + "maximum": 3 }, "initial_pose_offset_model_x": { "type": "array", @@ -19,9 +21,20 @@ "type": "array", "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", "default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + }, + "temperature": { + "type": "number", + "description": "In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance", + "default": 0.1, + "exclusiveMinimum": 0 } }, - "required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"], + + "required": [ + "covariance_estimation_type", + "initial_pose_offset_model_x", + "initial_pose_offset_model_y" + ], "additionalProperties": false } } diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 32e5a0f2a7c3c..012c72e6b3a46 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -82,7 +82,11 @@ void MapUpdateModule::callback_timer( bool MapUpdateModule::should_update_map( const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) { + last_update_position_mtx_.lock(); + if (last_update_position_ == std::nullopt) { + last_update_position_mtx_.unlock(); + need_rebuild_ = true; return true; } @@ -91,6 +95,8 @@ bool MapUpdateModule::should_update_map( const double dy = position.y - last_update_position_.value().y; const double distance = std::hypot(dx, dy); + last_update_position_mtx_.unlock(); + // check distance_last_update_position_to_current_position diagnostics_ptr->add_key_value("distance_last_update_position_to_current_position", distance); if (distance + param_.lidar_radius > param_.map_radius) { @@ -107,6 +113,27 @@ bool MapUpdateModule::should_update_map( return distance > param_.update_distance; } +bool MapUpdateModule::out_of_map_range(const geometry_msgs::msg::Point & position) +{ + last_update_position_mtx_.lock(); + + if (last_update_position_ == std::nullopt) { + last_update_position_mtx_.unlock(); + + return true; + } + + const double dx = position.x - last_update_position_.value().x; + const double dy = position.y - last_update_position_.value().y; + + last_update_position_mtx_.unlock(); + + const double distance = std::hypot(dx, dy); + + // check distance_last_update_position_to_current_position + return (distance + param_.lidar_radius > param_.map_radius); +} + void MapUpdateModule::update_map( const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) { @@ -140,8 +167,12 @@ void MapUpdateModule::update_map( diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str()); - last_update_position_ = position; ndt_ptr_mutex_->unlock(); + + last_update_position_mtx_.lock(); + last_update_position_ = position; + last_update_position_mtx_.unlock(); + return; } @@ -159,7 +190,10 @@ void MapUpdateModule::update_map( // check is_updated_map diagnostics_ptr->add_key_value("is_updated_map", updated); if (!updated) { + last_update_position_mtx_.lock(); last_update_position_ = position; + last_update_position_mtx_.unlock(); + return; } @@ -179,7 +213,9 @@ void MapUpdateModule::update_map( *secondary_ndt_ptr_ = *ndt_ptr_; // Memorize the position of the last update + last_update_position_mtx_.lock(); last_update_position_ = position; + last_update_position_mtx_.unlock(); // Publish the new ndt maps publish_partial_pcd_map(); diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index ca486a320d9dc..b394a8f863ceb 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -21,6 +21,7 @@ #include #include +#include #include @@ -439,6 +440,18 @@ bool NDTScanMatcher::callback_sensor_points_main( add_regularization_pose(sensor_ros_time); } + // Warn if the lidar has gone out of the map range + if (map_update_module_->out_of_map_range( + interpolation_result.interpolated_pose.pose.pose.position)) { + std::stringstream msg; + + msg << "Lidar has gone out of the map range"; + diagnostics_scan_points_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, msg.str()); + + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, msg.str()); + } + // check is_set_map_points const bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); diagnostics_scan_points_->add_key_value("is_set_map_points", is_set_map_points); @@ -566,11 +579,17 @@ bool NDTScanMatcher::callback_sensor_points_main( std::array ndt_covariance = rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation); - - if (is_converged && param_.covariance.covariance_estimation.enable) { - const auto estimated_covariance = + if ( + param_.covariance.covariance_estimation.covariance_estimation_type != + CovarianceEstimationType::FIXED_VALUE) { + const Eigen::Matrix2d estimated_covariance_2d = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); - ndt_covariance = estimated_covariance; + const Eigen::Matrix2d estimated_covariance_2d_adj = + pclomp::adjust_diagonal_covariance(estimated_covariance_2d, ndt_result.pose, 0.0225, 0.0225); + ndt_covariance[0 + 6 * 0] = estimated_covariance_2d_adj(0, 0); + ndt_covariance[1 + 6 * 1] = estimated_covariance_2d_adj(1, 1); + ndt_covariance[1 + 6 * 0] = estimated_covariance_2d_adj(1, 0); + ndt_covariance[0 + 6 * 1] = estimated_covariance_2d_adj(0, 1); } // check distance_initial_to_result @@ -807,7 +826,7 @@ int NDTScanMatcher::count_oscillation( return max_oscillation_cnt; } -std::array NDTScanMatcher::estimate_covariance( +Eigen::Matrix2d NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) { @@ -819,19 +838,9 @@ std::array NDTScanMatcher::estimate_covariance( std::stringstream message; message << "Error in Eigen solver: " << e.what(); RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - - return param_.covariance.output_pose_covariance; + return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0]; } - // first result is added to mean - const int n = - static_cast(param_.covariance.covariance_estimation.initial_pose_offset_model.size()) + 1; - const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); - Eigen::Vector2d mean = ndt_pose_2d; - std::vector ndt_pose_2d_vec; - ndt_pose_2d_vec.reserve(n); - ndt_pose_2d_vec.emplace_back(ndt_pose_2d); - geometry_msgs::msg::PoseArray multi_ndt_result_msg; geometry_msgs::msg::PoseArray multi_initial_pose_msg; multi_ndt_result_msg.header.stamp = sensor_ros_time; @@ -841,47 +850,45 @@ std::array NDTScanMatcher::estimate_covariance( multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose)); multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); - // multiple searches - for (const auto & pose_offset : - param_.covariance.covariance_estimation.initial_pose_offset_model) { - const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; - - Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); - sub_initial_pose_matrix = ndt_result.pose; - sub_initial_pose_matrix(0, 3) += static_cast(rotated_pose_offset_2d.x()); - sub_initial_pose_matrix(1, 3) += static_cast(rotated_pose_offset_2d.y()); - - auto sub_output_cloud = std::make_shared>(); - ndt_ptr_->align(*sub_output_cloud, sub_initial_pose_matrix); - const Eigen::Matrix4f sub_ndt_result = ndt_ptr_->getResult().pose; - - const Eigen::Vector2d sub_ndt_pose_2d = sub_ndt_result.topRightCorner<2, 1>().cast(); - mean += sub_ndt_pose_2d; - ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d); - - multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(sub_ndt_result)); - multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); - } - - // calculate the covariance matrix - mean /= n; - Eigen::Matrix2d pca_covariance = Eigen::Matrix2d::Zero(); - for (const auto & temp_ndt_pose_2d : ndt_pose_2d_vec) { - const Eigen::Vector2d diff_2d = temp_ndt_pose_2d - mean; - pca_covariance += diff_2d * diff_2d.transpose(); + if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::LAPLACE_APPROXIMATION) { + return pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian); + } else if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::MULTI_NDT) { + const std::vector poses_to_search = pclomp::propose_poses_to_search( + ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x, + param_.covariance.covariance_estimation.initial_pose_offset_model_y); + const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_multi_ndt_covariance_estimation = + estimate_xy_covariance_by_multi_ndt(ndt_result, ndt_ptr_, poses_to_search); + for (size_t i = 0; i < result_of_multi_ndt_covariance_estimation.ndt_initial_poses.size(); + i++) { + multi_ndt_result_msg.poses.push_back( + matrix4f_to_pose(result_of_multi_ndt_covariance_estimation.ndt_results[i].pose)); + multi_initial_pose_msg.poses.push_back( + matrix4f_to_pose(result_of_multi_ndt_covariance_estimation.ndt_initial_poses[i])); + } + multi_ndt_pose_pub_->publish(multi_ndt_result_msg); + multi_initial_pose_pub_->publish(multi_initial_pose_msg); + return result_of_multi_ndt_covariance_estimation.covariance; + } else if ( + param_.covariance.covariance_estimation.covariance_estimation_type == + CovarianceEstimationType::MULTI_NDT_SCORE) { + const std::vector poses_to_search = pclomp::propose_poses_to_search( + ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x, + param_.covariance.covariance_estimation.initial_pose_offset_model_y); + const pclomp::ResultOfMultiNdtCovarianceEstimation + result_of_multi_ndt_score_covariance_estimation = estimate_xy_covariance_by_multi_ndt_score( + ndt_result, ndt_ptr_, poses_to_search, param_.covariance.covariance_estimation.temperature); + for (const auto & sub_initial_pose_matrix : poses_to_search) { + multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); + } + multi_initial_pose_pub_->publish(multi_initial_pose_msg); + return result_of_multi_ndt_score_covariance_estimation.covariance; + } else { + return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0]; } - pca_covariance /= (n - 1); // unbiased covariance - - std::array ndt_covariance = param_.covariance.output_pose_covariance; - ndt_covariance[0 + 6 * 0] += pca_covariance(0, 0); - ndt_covariance[1 + 6 * 0] += pca_covariance(1, 0); - ndt_covariance[0 + 6 * 1] += pca_covariance(0, 1); - ndt_covariance[1 + 6 * 1] += pca_covariance(1, 1); - - multi_ndt_pose_pub_->publish(multi_ndt_result_msg); - multi_initial_pose_pub_->publish(multi_initial_pose_msg); - - return ndt_covariance; } void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index 0dbce08f309ac..158d8a8a8d283 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -4,7 +4,6 @@ twist2accel 0.0.0 The acceleration estimation package - Koji Minoda Yamato Ando Masahiro Sakamoto Kento Yabuuchi diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 46f5165c5f4e2..4c33377c8ed0f 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -53,14 +53,13 @@ class UndistortNode : public rclcpp::Node qos = rclcpp::QoS(10).durability_volatile().best_effort(); } - auto on_image = std::bind(&UndistortNode::on_image, this, _1); - auto on_compressed_image = std::bind(&UndistortNode::on_compressed_image, this, _1); - auto on_info = std::bind(&UndistortNode::on_info, this, _1); - sub_image_ = create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_image_ = create_subscription( + "~/input/image_raw", qos, std::bind(&UndistortNode::on_image, this, _1)); sub_compressed_image_ = create_subscription( - "~/input/image_raw/compressed", qos, std::move(on_compressed_image)); - - sub_info_ = create_subscription("~/input/camera_info", qos, std::move(on_info)); + "~/input/image_raw/compressed", qos, + std::bind(&UndistortNode::on_compressed_image, this, _1)); + sub_info_ = create_subscription( + "~/input/camera_info", qos, std::bind(&UndistortNode::on_info, this, _1)); pub_info_ = create_publisher("~/output/resized_info", 10); pub_image_ = create_publisher("~/output/resized_image", 10); diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml index a42a734dbab31..f3e6cfcdfbe5a 100644 --- a/localization/yabloc/yabloc_monitor/package.xml +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -5,7 +5,6 @@ 0.1.0 YabLoc monitor package Kento Yabuuchi - Koji Minoda Masahiro Sakamoto Yamato Ando NGUYEN Viet Anh diff --git a/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp b/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp index 95ca3f3dfbea0..46baffa270108 100644 --- a/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp +++ b/map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp @@ -31,8 +31,12 @@ using geometry_msgs::msg::Point; class MapHeightFitter final { public: - MapHeightFitter(rclcpp::Node * node); + explicit MapHeightFitter(rclcpp::Node * node); ~MapHeightFitter(); + MapHeightFitter(const MapHeightFitter &) = delete; + MapHeightFitter & operator=(const MapHeightFitter &) = delete; + MapHeightFitter(MapHeightFitter &&) = delete; + MapHeightFitter & operator=(MapHeightFitter &&) = delete; std::optional fit(const Point & position, const std::string & frame); private: diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index d6a20a636733d..c9e199ad88422 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -29,6 +29,8 @@ #include #include +#include + namespace map_height_fitter { @@ -107,7 +109,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n void MapHeightFitter::Impl::on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { map_frame_ = msg->header.frame_id; - map_cloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); + map_cloud_ = std::make_shared>(); pcl::fromROSMsg(*msg, *map_cloud_); } @@ -125,8 +127,8 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } const auto req = std::make_shared(); - req->area.center_x = point.x; - req->area.center_y = point.y; + req->area.center_x = static_cast(point.x); + req->area.center_y = static_cast(point.y); req->area.radius = 50; RCLCPP_DEBUG(logger, "Send request to map_loader"); @@ -157,7 +159,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } } map_frame_ = res->header.frame_id; - map_cloud_ = pcl::PointCloud::Ptr(new pcl::PointCloud); + map_cloud_ = std::make_shared>(); pcl::fromROSMsg(pcd_msg, *map_cloud_); return true; } @@ -276,9 +278,7 @@ MapHeightFitter::MapHeightFitter(rclcpp::Node * node) impl_ = std::make_unique(node); } -MapHeightFitter::~MapHeightFitter() -{ -} +MapHeightFitter::~MapHeightFitter() = default; std::optional MapHeightFitter::fit(const Point & position, const std::string & frame) { diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 48152605ec0a2..48d392a1b8e66 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + allow_unsupported_version: true # flag to load unsupported format_version anyway. If true, just prints warning. center_line_resolution: 5.0 # [m] use_waypoints: true # "centerline" in the Lanelet2 map will be used as a "waypoints" tag. lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 4e85ddec056c1..e38d65201ee56 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -15,6 +15,7 @@ #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#include #include #include #include @@ -27,18 +28,18 @@ #include #include -using autoware_map_msgs::msg::LaneletMapBin; -using tier4_map_msgs::msg::MapProjectorInfo; - class Lanelet2MapLoaderNode : public rclcpp::Node { +public: + static constexpr lanelet::autoware::Version version = lanelet::autoware::version; + public: explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static LaneletMapBin create_map_bin_msg( + static autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -48,7 +49,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp index cb640e4dc83d5..049d714ec452a 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -34,7 +34,7 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node bool viz_lanelets_centerline_; - void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); + void on_map_bin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); }; #endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 7c4d9eaf99556..6cceff98a8f9a 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -6,7 +6,6 @@ The map_loader package Yamato Ando Ryu Yamamoto - Koji Minoda Masahiro Sakamoto Kento Yabuuchi NGUYEN Viet Anh diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json index aae295f847ab2..a55050e4ed570 100644 --- a/map/map_loader/schema/lanelet2_map_loader.schema.json +++ b/map/map_loader/schema/lanelet2_map_loader.schema.json @@ -6,6 +6,11 @@ "lanelet2_map_loader": { "type": "object", "properties": { + "allow_unsupported_version": { + "type": "boolean", + "description": "Flag to load unsupported format_version anyway. If true, just prints warning.", + "default": "true" + }, "center_line_resolution": { "type": "number", "description": "Resolution of the Lanelet center line [m]", diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp index 225445d17bfa1..d73ec0d1ee06e 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp @@ -30,7 +30,7 @@ class LocalProjector : public Projector return BasicPoint3d{0.0, 0.0, gps.ele}; } - GPSPoint reverse(const BasicPoint3d & point) const override + [[nodiscard]] GPSPoint reverse(const BasicPoint3d & point) const override { return GPSPoint{0.0, 0.0, point.z()}; } diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 7b712c7c281a2..437c9e44cb7c1 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -49,8 +49,12 @@ #include #include +#include #include +using autoware_map_msgs::msg::LaneletMapBin; +using tier4_map_msgs::msg::MapProjectorInfo; + Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) : Node("lanelet2_map_loader", options) { @@ -61,6 +65,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); + declare_parameter("allow_unsupported_version"); declare_parameter("lanelet2_map_path"); declare_parameter("center_line_resolution"); declare_parameter("use_waypoints"); @@ -69,6 +74,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options void Lanelet2MapLoaderNode::on_map_projector_info( const MapProjectorInfo::Message::ConstSharedPtr msg) { + const auto allow_unsupported_version = get_parameter("allow_unsupported_version").as_bool(); const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); const auto use_waypoints = get_parameter("use_waypoints").as_bool(); @@ -80,6 +86,35 @@ void Lanelet2MapLoaderNode::on_map_projector_info( return; } + std::string format_version{"null"}, map_version{""}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + if (format_version == "null" || format_version.empty() || !isdigit(format_version[0])) { + RCLCPP_WARN( + get_logger(), + "%s has no format_version(null) or non semver-style format_version(%s) information", + lanelet2_filename.c_str(), format_version.c_str()); + if (!allow_unsupported_version) { + throw std::invalid_argument( + "allow_unsupported_version is false, so stop loading lanelet map"); + } + } else if (const auto map_major_ver_opt = lanelet::io_handlers::parseMajorVersion(format_version); + map_major_ver_opt.has_value()) { + const auto map_major_ver = map_major_ver_opt.value(); + if (map_major_ver > static_cast(lanelet::autoware::version)) { + RCLCPP_WARN( + get_logger(), + "format_version(%ld) of the provided map(%s) is larger than the supported version(%ld)", + map_major_ver, lanelet2_filename.c_str(), + static_cast(lanelet::autoware::version)); + if (!allow_unsupported_version) { + throw std::invalid_argument( + "allow_unsupported_version is false, so stop loading lanelet map"); + } + } + } + RCLCPP_INFO(get_logger(), "Loaded map format_version: %s", format_version.c_str()); + // overwrite centerline if (use_waypoints) { lanelet::utils::overwriteLaneletsCenterlineWithWaypoints(map, center_line_resolution, false); @@ -105,13 +140,13 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { std::unique_ptr projector = geography_utils::get_lanelet2_projector(projector_info); - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; } } else { const lanelet::projection::LocalProjector projector; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); if (!errors.empty()) { for (const auto & error : errors) { @@ -150,7 +185,8 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { - std::string format_version{}, map_version{}; + std::string format_version{}; + std::string map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 87319222001d9..4bff12c640c30 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -51,18 +51,18 @@ namespace { -void insertMarkerArray( +void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) { a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) +void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) { - cl->r = r; - cl->g = g; - cl->b = b; - cl->a = a; + cl->r = static_cast(r); + cl->g = static_cast(g); + cl->b = static_cast(b); + cl->a = static_cast(a); } } // namespace @@ -75,13 +75,13 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), - std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); + std::bind(&Lanelet2MapVisualizationNode::on_map_bin, this, _1)); pub_marker_ = this->create_publisher( "output/lanelet2_map_marker", rclcpp::QoS{1}.transient_local()); } -void Lanelet2MapVisualizationNode::onMapBin( +void Lanelet2MapVisualizationNode::on_map_bin( const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); @@ -103,8 +103,6 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); std::vector stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); - std::vector tl_reg_elems = - lanelet::utils::query::trafficLights(all_lanelets); std::vector aw_tl_reg_elems = lanelet::utils::query::autowareTrafficLights(all_lanelets); std::vector da_reg_elems = @@ -133,140 +131,158 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::utils::query::noParkingAreas(all_lanelets); lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); - std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, - cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, - cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, - cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, - cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones, cl_intersection_area; - setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); - setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); - setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); - setColor(&cl_partitions, 0.25, 0.25, 0.25, 0.999); - setColor(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); - setColor(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); - setColor(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); - setColor(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); - setColor(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); - setColor(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); - setColor(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); - setColor(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2); - setColor(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3); - setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); - setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); - setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); - setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); - setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); - setColor(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); + std_msgs::msg::ColorRGBA cl_road; + std_msgs::msg::ColorRGBA cl_shoulder; + std_msgs::msg::ColorRGBA cl_cross; + std_msgs::msg::ColorRGBA cl_partitions; + std_msgs::msg::ColorRGBA cl_pedestrian_markings; + std_msgs::msg::ColorRGBA cl_ll_borders; + std_msgs::msg::ColorRGBA cl_shoulder_borders; + std_msgs::msg::ColorRGBA cl_stoplines; + std_msgs::msg::ColorRGBA cl_trafficlights; + std_msgs::msg::ColorRGBA cl_detection_areas; + std_msgs::msg::ColorRGBA cl_speed_bumps; + std_msgs::msg::ColorRGBA cl_crosswalks; + std_msgs::msg::ColorRGBA cl_parking_lots; + std_msgs::msg::ColorRGBA cl_parking_spaces; + std_msgs::msg::ColorRGBA cl_lanelet_id; + std_msgs::msg::ColorRGBA cl_obstacle_polygons; + std_msgs::msg::ColorRGBA cl_no_stopping_areas; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area_for_run_out; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_area; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_line; + std_msgs::msg::ColorRGBA cl_no_parking_areas; + std_msgs::msg::ColorRGBA cl_curbstones; + std_msgs::msg::ColorRGBA cl_intersection_area; + set_color(&cl_road, 0.27, 0.27, 0.27, 0.999); + set_color(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); + set_color(&cl_cross, 0.27, 0.3, 0.27, 0.5); + set_color(&cl_partitions, 0.25, 0.25, 0.25, 0.999); + set_color(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); + set_color(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); + set_color(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); + set_color(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); + set_color(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); + set_color(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); + set_color(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); + set_color(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2); + set_color(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3); + set_color(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); + set_color(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); + set_color(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); + set_color(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + set_color(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); + set_color(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(partitions, "partitions", cl_partitions, 0.1)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( pedestrian_polygon_markings, cl_pedestrian_markings)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( pedestrian_line_markings, cl_pedestrian_markings)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "walkway_lanelets", walkway_lanelets, cl_cross)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::obstaclePolygonsAsMarkerArray(obstacle_polygons, cl_obstacle_polygons)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( shoulder_lanelets, cl_shoulder_borders, viz_lanelets_centerline_, "shoulder_")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( road_lanelets, cl_ll_borders, viz_lanelets_centerline_)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( road_lanelets, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( crosswalk_lanelets, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(shoulder_lanelets, cl_lanelet_id)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::generateLaneletIdMarker( crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray( no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); - insertMarkerArray( + insert_marker_array( &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( intersection_areas, cl_intersection_area)); diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index a8d380fd81b86..da42389dcc69f 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -14,20 +14,22 @@ #include "differential_map_loader_module.hpp" +#include + DifferentialMapLoaderModule::DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_differential_pcd_maps_service_ = node->create_service( "service/get_differential_pcd_map", std::bind( - &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, + &DifferentialMapLoaderModule::on_service_get_differential_point_cloud_map, this, std::placeholders::_1, std::placeholders::_2)); } -void DifferentialMapLoaderModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo & area, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const +void DifferentialMapLoaderModule::differential_area_load( + const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, + const GetDifferentialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids std::vector should_remove(static_cast(cached_ids.size()), true); @@ -36,18 +38,18 @@ void DifferentialMapLoaderModule::differentialAreaLoad( PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = path; // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; + if (!is_grid_within_queried_area(area_info, metadata)) continue; auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); if (id_in_cached_list != cached_ids.end()) { - int index = id_in_cached_list - cached_ids.begin(); + int index = static_cast(id_in_cached_list - cached_ids.begin()); should_remove[index] = false; } else { autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -63,19 +65,19 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } } -bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( +bool DifferentialMapLoaderModule::on_service_get_differential_point_cloud_map( GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res) + GetDifferentialPointCloudMap::Response::SharedPtr res) const { auto area = req->area; std::vector cached_ids = req->cached_ids; - differentialAreaLoad(area, cached_ids, res); + differential_area_load(area, cached_ids, res); res->header.frame_id = "map"; return true; } autoware_map_msgs::msg::PointCloudMapCellWithID -DifferentialMapLoaderModule::loadPointCloudMapCellWithID( +DifferentialMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 7069e1dbdf45c..690ffeca653c8 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -38,7 +38,7 @@ class DifferentialMapLoaderModule public: explicit DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -46,13 +46,13 @@ class DifferentialMapLoaderModule std::map all_pcd_file_metadata_dict_; rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; - bool onServiceGetDifferentialPointCloudMap( + [[nodiscard]] bool on_service_get_differential_point_cloud_map( GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res); - void differentialAreaLoad( + GetDifferentialPointCloudMap::Response::SharedPtr res) const; + void differential_area_load( const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const GetDifferentialPointCloudMap::Response::SharedPtr & response) const; + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 8c9378e9dfadb..62e165dd1005b 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -14,19 +14,22 @@ #include "partial_map_loader_module.hpp" +#include + PartialMapLoaderModule::PartialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_partial_pcd_maps_service_ = node->create_service( - "service/get_partial_pcd_map", std::bind( - &PartialMapLoaderModule::onServiceGetPartialPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); + "service/get_partial_pcd_map", + std::bind( + &PartialMapLoaderModule::on_service_get_partial_point_cloud_map, this, std::placeholders::_1, + std::placeholders::_2)); } -void PartialMapLoaderModule::partialAreaLoad( +void PartialMapLoaderModule::partial_area_load( const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const + const GetPartialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids for (const auto & ele : all_pcd_file_metadata_dict_) { @@ -34,13 +37,13 @@ void PartialMapLoaderModule::partialAreaLoad( PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = path; // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; + if (!is_grid_within_queried_area(area, metadata)) continue; autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -50,16 +53,18 @@ void PartialMapLoaderModule::partialAreaLoad( } } -bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( - GetPartialPointCloudMap::Request::SharedPtr req, GetPartialPointCloudMap::Response::SharedPtr res) +bool PartialMapLoaderModule::on_service_get_partial_point_cloud_map( + GetPartialPointCloudMap::Request::SharedPtr req, + GetPartialPointCloudMap::Response::SharedPtr res) const { auto area = req->area; - partialAreaLoad(area, res); + partial_area_load(area, res); res->header.frame_id = "map"; return true; } -autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( +autoware_map_msgs::msg::PointCloudMapCellWithID +PartialMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 4d97ab90667ec..ec97661366419 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -38,7 +38,7 @@ class PartialMapLoaderModule public: explicit PartialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -46,13 +46,13 @@ class PartialMapLoaderModule std::map all_pcd_file_metadata_dict_; rclcpp::Service::SharedPtr get_partial_pcd_maps_service_; - bool onServiceGetPartialPointCloudMap( + [[nodiscard]] bool on_service_get_partial_point_cloud_map( GetPartialPointCloudMap::Request::SharedPtr req, - GetPartialPointCloudMap::Response::SharedPtr res); - void partialAreaLoad( + GetPartialPointCloudMap::Response::SharedPtr res) const; + void partial_area_load( const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + const GetPartialPointCloudMap::Response::SharedPtr & response) const; + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index a66f9ee99534c..1754d0b7629a2 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -50,10 +50,10 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule( sensor_msgs::msg::PointCloud2 pcd; if (use_downsample) { - const float leaf_size = node->declare_parameter("leaf_size"); - pcd = loadPCDFiles(pcd_paths, leaf_size); + const float leaf_size = static_cast(node->declare_parameter("leaf_size")); + pcd = load_pcd_files(pcd_paths, leaf_size); } else { - pcd = loadPCDFiles(pcd_paths, boost::none); + pcd = load_pcd_files(pcd_paths, boost::none); } if (pcd.width == 0) { @@ -65,7 +65,7 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule( pub_pointcloud_map_->publish(pcd); } -sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( +sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::load_pcd_files( const std::vector & pcd_paths, const boost::optional leaf_size) const { sensor_msgs::msg::PointCloud2 whole_pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 6a87643b57bff..44f23ded70e37 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -39,7 +39,7 @@ class PointcloudMapLoaderModule rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - sensor_msgs::msg::PointCloud2 loadPCDFiles( + [[nodiscard]] sensor_msgs::msg::PointCloud2 load_pcd_files( const std::vector & pcd_paths, const boost::optional leaf_size) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 349fc2954fe28..c718b25c56694 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -28,7 +28,7 @@ namespace fs = std::filesystem; namespace { -bool isPcdFile(const std::string & p) +bool is_pcd_file(const std::string & p) { if (fs::is_directory(p)) { return false; @@ -36,11 +36,7 @@ bool isPcdFile(const std::string & p) const std::string ext = fs::path(p).extension(); - if (ext != ".pcd" && ext != ".PCD") { - return false; - } - - return true; + return !(ext != ".pcd" && ext != ".PCD"); } } // namespace @@ -48,7 +44,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt : Node("pointcloud_map_loader", options) { const auto pcd_paths = - getPcdPaths(declare_parameter>("pcd_paths_or_directory")); + get_pcd_paths(declare_parameter>("pcd_paths_or_directory")); std::string pcd_metadata_path = declare_parameter("pcd_metadata_path"); bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); @@ -68,7 +64,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } // Parse the metadata file and get the map of (absolute pcd path, pcd file metadata) - auto pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + auto pcd_metadata_dict = get_pcd_metadata(pcd_metadata_path, pcd_paths); if (enable_partial_load) { partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); @@ -81,14 +77,14 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } } -std::map PointCloudMapLoaderNode::getPCDMetadata( +std::map PointCloudMapLoaderNode::get_pcd_metadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const { if (fs::exists(pcd_metadata_path)) { std::set missing_pcd_names; - auto pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); + auto pcd_metadata_dict = load_pcd_metadata(pcd_metadata_path); - pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths, missing_pcd_names); + pcd_metadata_dict = replace_with_absolute_path(pcd_metadata_dict, pcd_paths, missing_pcd_names); // Warning if some segments are missing if (!missing_pcd_names.empty()) { @@ -96,7 +92,7 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( oss << "The following segment(s) are missing from the input PCDs: "; - for (auto & fname : missing_pcd_names) { + for (const auto & fname : missing_pcd_names) { oss << std::endl << fname; } @@ -105,7 +101,9 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( } return pcd_metadata_dict; - } else if (pcd_paths.size() == 1) { + } + + if (pcd_paths.size() == 1) { // An exception when using a single file PCD map so that the users do not have to provide // a metadata file. // Note that this should ideally be avoided and thus eventually be removed by someone, until @@ -119,12 +117,11 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( PCDFileMetadata metadata = {}; pcl::getMinMax3D(single_pcd, metadata.min, metadata.max); return std::map{{pcd_path, metadata}}; - } else { - throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); } + throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); } -std::vector PointCloudMapLoaderNode::getPcdPaths( +std::vector PointCloudMapLoaderNode::get_pcd_paths( const std::vector & pcd_paths_or_directory) const { std::vector pcd_paths; @@ -133,14 +130,14 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p); } - if (isPcdFile(p)) { + if (is_pcd_file(p)) { pcd_paths.push_back(p); } if (fs::is_directory(p)) { for (const auto & file : fs::directory_iterator(p)) { const auto filename = file.path().string(); - if (isPcdFile(filename)) { + if (is_pcd_file(filename)) { pcd_paths.push_back(filename); } } diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index fd9b297a9e3f4..dbc0d584d347b 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -45,9 +45,9 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::unique_ptr differential_map_loader_; std::unique_ptr selected_map_loader_; - std::vector getPcdPaths( + std::vector get_pcd_paths( const std::vector & pcd_paths_or_directory) const; - std::map getPCDMetadata( + std::map get_pcd_metadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp index 3e7b046f9d178..76b56341b8632 100644 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp @@ -13,9 +13,11 @@ // limitations under the License. #include "selected_map_loader_module.hpp" + +#include namespace { -autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( +autoware_map_msgs::msg::PointCloudMapMetaData create_metadata( const std::map & pcd_file_metadata_dict) { autoware_map_msgs::msg::PointCloudMapMetaData metadata_msg; @@ -23,11 +25,10 @@ autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( metadata_msg.header.stamp = rclcpp::Clock().now(); for (const auto & ele : pcd_file_metadata_dict) { - std::string path = ele.first; PCDFileMetadata metadata = ele.second; // assume that the map ID = map path (for now) - std::string map_id = path; + const std::string & map_id = ele.first; autoware_map_msgs::msg::PointCloudMapCellMetaDataWithID cell_metadata_with_id; cell_metadata_with_id.cell_id = map_id; @@ -44,23 +45,24 @@ autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( } // namespace SelectedMapLoaderModule::SelectedMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) + rclcpp::Node * node, std::map pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(std::move(pcd_file_metadata_dict)) { get_selected_pcd_maps_service_ = node->create_service( - "service/get_selected_pcd_map", std::bind( - &SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); + "service/get_selected_pcd_map", + std::bind( + &SelectedMapLoaderModule::on_service_get_selected_point_cloud_map, this, + std::placeholders::_1, std::placeholders::_2)); // publish the map metadata rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); pub_metadata_ = node->create_publisher( "output/pointcloud_map_metadata", durable_qos); - pub_metadata_->publish(createMetadata(all_pcd_file_metadata_dict_)); + pub_metadata_->publish(create_metadata(all_pcd_file_metadata_dict_)); } -bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( +bool SelectedMapLoaderModule::on_service_get_selected_point_cloud_map( GetSelectedPointCloudMap::Request::SharedPtr req, GetSelectedPointCloudMap::Response::SharedPtr res) const { @@ -76,11 +78,11 @@ bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( const std::string path = requested_selected_map_iterator->first; // assume that the map ID = map path (for now) - const std::string map_id = path; + const std::string & map_id = path; PCDFileMetadata metadata = requested_selected_map_iterator->second; autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); + load_point_cloud_map_cell_with_id(path, map_id); pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; @@ -93,7 +95,7 @@ bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( } autoware_map_msgs::msg::PointCloudMapCellWithID -SelectedMapLoaderModule::loadPointCloudMapCellWithID( +SelectedMapLoaderModule::load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp index f44d549a0f576..eea8b8c1950ae 100644 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp @@ -39,7 +39,7 @@ class SelectedMapLoaderModule public: explicit SelectedMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + rclcpp::Node * node, std::map pcd_file_metadata_dict); private: rclcpp::Logger logger_; @@ -49,10 +49,10 @@ class SelectedMapLoaderModule rclcpp::Publisher::SharedPtr pub_metadata_; - bool onServiceGetSelectedPointCloudMap( + [[nodiscard]] bool on_service_get_selected_point_cloud_map( GetSelectedPointCloudMap::Request::SharedPtr req, GetSelectedPointCloudMap::Response::SharedPtr res) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id( const std::string & path, const std::string & map_id) const; }; diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 96f9d114ed265..ea2c2e7033014 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -20,7 +20,7 @@ #include #include -std::map loadPCDMetadata(const std::string & pcd_metadata_path) +std::map load_pcd_metadata(const std::string & pcd_metadata_path) { YAML::Node config = YAML::LoadFile(pcd_metadata_path); @@ -33,22 +33,22 @@ std::map loadPCDMetadata(const std::string & pcd_m continue; } - std::string key = node.first.as(); - std::vector values = node.second.as>(); + auto key = node.first.as(); + auto values = node.second.as>(); - PCDFileMetadata fileMetadata; - fileMetadata.min.x = values[0]; - fileMetadata.min.y = values[1]; - fileMetadata.max.x = values[0] + config["x_resolution"].as(); - fileMetadata.max.y = values[1] + config["y_resolution"].as(); + PCDFileMetadata file_metadata; + file_metadata.min.x = static_cast(values[0]); + file_metadata.min.y = static_cast(values[1]); + file_metadata.max.x = static_cast(values[0]) + config["x_resolution"].as(); + file_metadata.max.y = static_cast(values[1]) + config["y_resolution"].as(); - metadata[key] = fileMetadata; + metadata[key] = file_metadata; } return metadata; } -std::map replaceWithAbsolutePath( +std::map replace_with_absolute_path( const std::map & pcd_metadata_path, const std::vector & pcd_paths, std::set & missing_pcd_names) { @@ -75,7 +75,7 @@ std::map replaceWithAbsolutePath( return absolute_path_map; } -bool cylinderAndBoxOverlapExists( +bool cylinder_and_box_overlap_exists( const double center_x, const double center_y, const double radius, const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point) { @@ -92,22 +92,18 @@ bool cylinderAndBoxOverlapExists( const double dy0 = center_y - box_min_point.y; const double dy1 = center_y - box_max_point.y; - if ( - std::hypot(dx0, dy0) <= radius || std::hypot(dx1, dy0) <= radius || - std::hypot(dx0, dy1) <= radius || std::hypot(dx1, dy1) <= radius) { - return true; - } - - return false; + return std::hypot(dx0, dy0) <= radius || std::hypot(dx1, dy0) <= radius || + std::hypot(dx0, dy1) <= radius || std::hypot(dx1, dy1) <= radius; } -bool isGridWithinQueriedArea( +bool is_grid_within_queried_area( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) { // Currently, the area load only supports cylindrical area double center_x = area.center_x; double center_y = area.center_y; double radius = area.radius; - bool res = cylinderAndBoxOverlapExists(center_x, center_y, radius, metadata.min, metadata.max); + bool res = + cylinder_and_box_overlap_exists(center_x, center_y, radius, metadata.min, metadata.max); return res; } diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 29d9a24d7b0e7..07e8ade5c6f7c 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -37,15 +37,15 @@ struct PCDFileMetadata } }; -std::map loadPCDMetadata(const std::string & pcd_metadata_path); -std::map replaceWithAbsolutePath( +std::map load_pcd_metadata(const std::string & pcd_metadata_path); +std::map replace_with_absolute_path( const std::map & pcd_metadata_path, const std::vector & pcd_paths, std::set & missing_pcd_names); -bool cylinderAndBoxOverlapExists( +bool cylinder_and_box_overlap_exists( const double center_x, const double center_y, const double radius, - const pcl::PointXYZ position_min, const pcl::PointXYZ position_max); -bool isGridWithinQueriedArea( + const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point); +bool is_grid_within_queried_area( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata); #endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py index 9f9a59f565e3f..0a29a74e90b06 100644 --- a/map/map_loader/test/lanelet2_map_loader_launch.test.py +++ b/map/map_loader/test/lanelet2_map_loader_launch.test.py @@ -39,6 +39,7 @@ def generate_test_description(): "lanelet2_map_path": lanelet2_map_path, "center_line_resolution": 5.0, "use_waypoints": True, + "allow_unsupported_version": True, } ], ) diff --git a/map/map_loader/test/test_cylinder_box_overlap.cpp b/map/map_loader/test/test_cylinder_box_overlap.cpp index 4c9b18a3dbd8e..d8ca2ca9f8734 100644 --- a/map/map_loader/test/test_cylinder_box_overlap.cpp +++ b/map/map_loader/test/test_cylinder_box_overlap.cpp @@ -35,7 +35,7 @@ TEST(CylinderAndBoxOverlapExists, NoOverlap) p_max.y = 1.0; p_max.z = 1.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_FALSE(result); } @@ -57,7 +57,7 @@ TEST(CylinderAndBoxOverlapExists, Overlap1) p_max.y = 1.0; p_max.z = 1.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_TRUE(result); } @@ -79,6 +79,6 @@ TEST(CylinderAndBoxOverlapExists, Overlap2) p_max.y = 1.0; p_max.z = -99.0; - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); + bool result = cylinder_and_box_overlap_exists(center_x, center_y, radius, p_min, p_max); EXPECT_TRUE(result); } diff --git a/map/map_loader/test/test_load_pcd_metadata.cpp b/map/map_loader/test/test_load_pcd_metadata.cpp index a832489b6db99..fcec100f389c5 100644 --- a/map/map_loader/test/test_load_pcd_metadata.cpp +++ b/map/map_loader/test/test_load_pcd_metadata.cpp @@ -21,7 +21,7 @@ using ::testing::ContainerEq; -std::string createYAMLFile() +std::string create_yaml_file() { std::filesystem::path tmp_path = std::filesystem::temp_directory_path() / "temp_metadata.yaml"; @@ -37,14 +37,14 @@ std::string createYAMLFile() TEST(LoadPCDMetadataTest, BasicFunctionality) { - std::string yaml_file_path = createYAMLFile(); + std::string yaml_file_path = create_yaml_file(); std::map expected = { {"file1.pcd", {{1, 2, 0}, {6, 8, 0}}}, {"file2.pcd", {{3, 4, 0}, {8, 10, 0}}}, }; - auto result = loadPCDMetadata(yaml_file_path); + auto result = load_pcd_metadata(yaml_file_path); ASSERT_THAT(result, ContainerEq(expected)); } diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp index 2b686dc0fe8c3..5667f476b4dab 100644 --- a/map/map_loader/test/test_pointcloud_map_loader_module.cpp +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -26,8 +26,6 @@ #include #include -using std::chrono_literals::operator""ms; - class TestPointcloudMapLoaderModule : public ::testing::Test { protected: @@ -61,6 +59,8 @@ class TestPointcloudMapLoaderModule : public ::testing::Test TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) { + using namespace std::literals::chrono_literals; + // Prepare PCD paths std::vector pcd_paths = {temp_pcd_path}; diff --git a/map/map_loader/test/test_replace_with_absolute_path.cpp b/map/map_loader/test/test_replace_with_absolute_path.cpp index f61dd188f0679..03d533d41cf18 100644 --- a/map/map_loader/test/test_replace_with_absolute_path.cpp +++ b/map/map_loader/test/test_replace_with_absolute_path.cpp @@ -37,7 +37,7 @@ TEST(ReplaceWithAbsolutePathTest, BasicFunctionality) }; std::set missing_pcd_names; - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names); + auto result = replace_with_absolute_path(pcd_metadata_path, pcd_paths, missing_pcd_names); ASSERT_THAT(result, ContainerEq(expected)); } @@ -56,7 +56,7 @@ TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles) std::map expected = {}; std::set missing_pcd_names; - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names); + auto result = replace_with_absolute_path(pcd_metadata_path, pcd_paths, missing_pcd_names); ASSERT_THAT(result, ContainerEq(expected)); } diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index b77ef18ac2293..c36c22c29a0fd 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -4,7 +4,6 @@ map_projection_loader 0.1.0 map_projection_loader package as a ROS 2 node - Koji Minoda Yamato Ando Masahiro Sakamoto Kento Yabuuchi diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index cbd279af2b46f..571c2e91c53a5 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -27,10 +27,7 @@ #include #include -using lanelet::utils::getId; -using lanelet::utils::to2D; - -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -48,32 +45,32 @@ bool loadLaneletMap( return true; } -lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Lanelets convert_to_vector(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Lanelets lanelets; - for (lanelet::Lanelet lanelet : lanelet_map_ptr->laneletLayer) { - lanelets.push_back(lanelet); - } + std::copy( + lanelet_map_ptr->laneletLayer.begin(), lanelet_map_ptr->laneletLayer.end(), + std::back_inserter(lanelets)); return lanelets; } -void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr) +void fix_tags(lanelet::LaneletMapPtr & lanelet_map_ptr) { - auto lanelets = convertToVector(lanelet_map_ptr); - lanelet::traffic_rules::TrafficRulesPtr trafficRules = + auto lanelets = convert_to_vector(lanelet_map_ptr); + lanelet::traffic_rules::TrafficRulesPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle); - lanelet::routing::RoutingGraphUPtr routingGraph = - lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *trafficRules); + lanelet::routing::RoutingGraphUPtr routing_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *traffic_rules); for (auto & llt : lanelets) { - if (!routingGraph->conflicting(llt).empty()) { + if (!routing_graph->conflicting(llt).empty()) { continue; } llt.attributes().erase("turn_direction"); - if (!!routingGraph->adjacentRight(llt)) { + if (!!routing_graph->adjacentRight(llt)) { llt.rightBound().attributes()["lane_change"] = "yes"; } - if (!!routingGraph->adjacentLeft(llt)) { + if (!!routing_graph->adjacentLeft(llt)) { llt.leftBound().attributes()["lane_change"] = "yes"; } } @@ -91,11 +88,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - fixTags(llt_map_ptr); + fix_tags(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp index 14a33b01beee0..af565208c5f71 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -27,7 +27,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -45,7 +45,8 @@ bool loadLaneletMap( return true; } -bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +bool load_pcd_map( + const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); @@ -56,16 +57,16 @@ bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud return true; } -double getMinHeightAroundPoint( +double get_min_height_around_point( const pcl::PointCloud::Ptr & pcd_map_ptr, const pcl::KdTreeFLANN & kdtree, const pcl::PointXYZ & search_pt, const double search_radius3d, const double search_radius2d) { - std::vector pointIdxRadiusSearch; - std::vector pointRadiusSquaredDistance; + std::vector point_idx_radius_search; + std::vector point_radius_squared_distance; if ( kdtree.radiusSearch( - search_pt, search_radius3d, pointIdxRadiusSearch, pointRadiusSquaredDistance) <= 0) { + search_pt, search_radius3d, point_idx_radius_search, point_radius_squared_distance) <= 0) { std::cout << "no points found within 3d radius " << search_radius3d << std::endl; return search_pt.z; } @@ -73,8 +74,7 @@ double getMinHeightAroundPoint( double min_height = std::numeric_limits::max(); bool found = false; - for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); i++) { - std::size_t pt_idx = pointIdxRadiusSearch.at(i); + for (auto pt_idx : point_idx_radius_search) { const auto pt = pcd_map_ptr->points.at(pt_idx); if (pt.z > min_height) { continue; @@ -91,8 +91,9 @@ double getMinHeightAroundPoint( return min_height; } -void adjustHeight( - const pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr) +void adjust_height( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const lanelet::LaneletMapPtr & lanelet_map_ptr) { std::unordered_set done; double search_radius2d = 0.5; @@ -107,11 +108,11 @@ void adjustHeight( continue; } pcl::PointXYZ pcl_pt; - pcl_pt.x = pt.x(); - pcl_pt.y = pt.y(); - pcl_pt.z = pt.z(); + pcl_pt.x = static_cast(pt.x()); + pcl_pt.y = static_cast(pt.y()); + pcl_pt.z = static_cast(pt.z()); double min_height = - getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + get_min_height_around_point(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; pt.z() = min_height; done.insert(pt.id()); @@ -121,11 +122,11 @@ void adjustHeight( continue; } pcl::PointXYZ pcl_pt; - pcl_pt.x = pt.x(); - pcl_pt.y = pt.y(); - pcl_pt.z = pt.z(); + pcl_pt.x = static_cast(pt.x()); + pcl_pt.y = static_cast(pt.y()); + pcl_pt.z = static_cast(pt.z()); double min_height = - getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + get_min_height_around_point(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; pt.z() = min_height; done.insert(pt.id()); @@ -148,14 +149,14 @@ int main(int argc, char * argv[]) pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { return EXIT_FAILURE; } - adjustHeight(pcd_map_ptr, llt_map_ptr); + adjust_height(pcd_map_ptr, llt_map_ptr); lanelet::write(llt_output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp index e0d071ea11dc1..0e40d04a2cec3 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -26,10 +26,7 @@ #include #include -using lanelet::utils::getId; -using lanelet::utils::to2D; - -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -47,21 +44,17 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return set.find(element) != set.end(); -} - -lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::LineStrings3d convert_line_layer_to_line_strings( + const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LineStrings3d lines; - for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { - lines.push_back(line); - } + std::copy( + lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), + std::back_inserter(lines)); return lines; } -lanelet::ConstPoint3d get3DPointFrom2DArcLength( +lanelet::ConstPoint3d get3d_point_from2d_arc_length( const lanelet::ConstLineString3d & line, const double s) { double accumulated_distance2d = 0; @@ -71,27 +64,23 @@ lanelet::ConstPoint3d get3DPointFrom2DArcLength( auto prev_pt = line.front(); for (size_t i = 1; i < line.size(); i++) { const auto & pt = line[i]; - double distance2d = lanelet::geometry::distance2d(to2D(prev_pt), to2D(pt)); + double distance2d = + lanelet::geometry::distance2d(lanelet::utils::to2D(prev_pt), lanelet::utils::to2D(pt)); if (accumulated_distance2d + distance2d >= s) { double ratio = (s - accumulated_distance2d) / distance2d; auto interpolated_pt = prev_pt.basicPoint() * (1 - ratio) + pt.basicPoint() * ratio; std::cout << interpolated_pt << std::endl; - return lanelet::ConstPoint3d( - lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + return lanelet::ConstPoint3d{ + lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()}; } accumulated_distance2d += distance2d; prev_pt = pt; } RCLCPP_ERROR(rclcpp::get_logger("merge_close_lines"), "interpolation failed"); - return lanelet::ConstPoint3d(); -} - -double getLineLength(const lanelet::ConstLineString3d & line) -{ - return boost::geometry::length(line.basicLineString()); + return {}; } -bool areLinesSame( +bool are_lines_same( const lanelet::ConstLineString3d & line1, const lanelet::ConstLineString3d & line2) { bool same_ends = false; @@ -105,66 +94,63 @@ bool areLinesSame( return false; } - double sum_distance = 0; - for (const auto & pt : line1) { - sum_distance += boost::geometry::distance(pt.basicPoint(), line2); - } - for (const auto & pt : line2) { - sum_distance += boost::geometry::distance(pt.basicPoint(), line1); - } + double sum_distance = + std::accumulate(line1.begin(), line1.end(), 0.0, [&line2](double sum, const auto & pt) { + return sum + boost::geometry::distance(pt.basicPoint(), line2); + }); + sum_distance += + std::accumulate(line2.begin(), line2.end(), 0.0, [&line1](double sum, const auto & pt) { + return sum + boost::geometry::distance(pt.basicPoint(), line1); + }); - double avg_distance = sum_distance / (line1.size() + line2.size()); + double avg_distance = sum_distance / static_cast(line1.size() + line2.size()); std::cout << line1 << " " << line2 << " " << avg_distance << std::endl; - if (avg_distance < 1.0) { - return true; - } else { - return false; - } + return avg_distance < 1.0; } -lanelet::BasicPoint3d getClosestPointOnLine( +lanelet::BasicPoint3d get_closest_point_on_line( const lanelet::BasicPoint3d & search_point, const lanelet::ConstLineString3d & line) { - auto arc_coordinate = lanelet::geometry::toArcCoordinates(to2D(line), to2D(search_point)); + auto arc_coordinate = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(line), lanelet::utils::to2D(search_point)); std::cout << arc_coordinate.length << " " << arc_coordinate.distance << std::endl; - return get3DPointFrom2DArcLength(line, arc_coordinate.length).basicPoint(); + return get3d_point_from2d_arc_length(line, arc_coordinate.length).basicPoint(); } -lanelet::LineString3d mergeTwoLines( +lanelet::LineString3d merge_two_lines( const lanelet::LineString3d & line1, const lanelet::ConstLineString3d & line2) { lanelet::Points3d new_points; for (const auto & p1 : line1) { - lanelet::BasicPoint3d p1_basic_point = p1.basicPoint(); - lanelet::BasicPoint3d p2_basic_point = getClosestPointOnLine(p1, line2); + const lanelet::BasicPoint3d & p1_basic_point = p1.basicPoint(); + lanelet::BasicPoint3d p2_basic_point = get_closest_point_on_line(p1, line2); lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point) / 2; lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); new_points.push_back(new_point); } - return lanelet::LineString3d(lanelet::utils::getId(), new_points); + return lanelet::LineString3d{lanelet::utils::getId(), new_points}; } -void copyData(lanelet::LineString3d & dst, lanelet::LineString3d & src) +void copy_data(lanelet::LineString3d & dst, const lanelet::LineString3d & src) { - lanelet::Points3d points; dst.clear(); - for (lanelet::Point3d & pt : src) { - dst.push_back(pt); + for (const lanelet::ConstPoint3d & pt : src) { + dst.push_back(static_cast(pt)); } } -void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr) +void merge_lines(lanelet::LaneletMapPtr & lanelet_map_ptr) { - auto lines = convertLineLayerToLineStrings(lanelet_map_ptr); + auto lines = convert_line_layer_to_line_strings(lanelet_map_ptr); for (size_t i = 0; i < lines.size(); i++) { auto line_i = lines.at(i); for (size_t j = 0; j < i; j++) { auto line_j = lines.at(j); - if (areLinesSame(line_i, line_j)) { - auto merged_line = mergeTwoLines(line_i, line_j); - copyData(line_i, merged_line); - copyData(line_j, merged_line); + if (are_lines_same(line_i, line_j)) { + auto merged_line = merge_two_lines(line_i, line_j); + copy_data(line_i, merged_line); + copy_data(line_j, merged_line); line_i.setId(line_j.id()); std::cout << line_j << " " << line_i << std::endl; // lanelet_map_ptr->add(merged_line); @@ -189,11 +175,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - mergeLines(llt_map_ptr); + merge_lines(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp index e18a366003e10..3cbbffc702019 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -25,7 +25,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -43,17 +43,12 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return set.find(element) != set.end(); -} - -lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; - for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - points.push_back(pt); - } + std::copy( + lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), + std::back_inserter(points)); return points; } @@ -72,9 +67,9 @@ lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_ma // return lanelet::LineString3d(lanelet::utils::getId(), new_points); // } -void mergePoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +void merge_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { - const auto & points = convertPointsLayerToPoints(lanelet_map_ptr); + const auto & points = convert_points_layer_to_points(lanelet_map_ptr); for (size_t i = 0; i < points.size(); i++) { auto point_i = points.at(i); @@ -109,11 +104,11 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - mergePoints(llt_map_ptr); + merge_points(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp index 633dae4e20c1d..3ab10551e36f9 100644 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -25,7 +25,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -43,28 +43,29 @@ bool loadLaneletMap( return true; } -lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; - for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - points.push_back(pt); - } + std::copy( + lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), + std::back_inserter(points)); return points; } -lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +lanelet::LineStrings3d convert_line_layer_to_line_strings( + const lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LineStrings3d lines; - for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { - lines.push_back(line); - } + std::copy( + lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), + std::back_inserter(lines)); return lines; } -void removeUnreferencedGeometry(lanelet::LaneletMapPtr & lanelet_map_ptr) +void remove_unreferenced_geometry(lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap); - for (auto llt : lanelet_map_ptr->laneletLayer) { + for (const auto & llt : lanelet_map_ptr->laneletLayer) { new_map->add(llt); } lanelet_map_ptr = new_map; @@ -82,10 +83,10 @@ int main(int argc, char * argv[]) lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - removeUnreferencedGeometry(llt_map_ptr); + remove_unreferenced_geometry(llt_map_ptr); lanelet::write(output_path, *llt_map_ptr, projector); rclcpp::shutdown(); diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp index d29df716b11e7..d55e03c7faee5 100644 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -27,7 +27,7 @@ #include #include -bool loadLaneletMap( +bool load_lanelet_map( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) { @@ -45,7 +45,8 @@ bool loadLaneletMap( return true; } -bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +bool load_pcd_map( + const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); @@ -56,9 +57,9 @@ bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud return true; } -void transformMaps( - pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr, - const Eigen::Affine3d affine) +void transform_maps( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const Eigen::Affine3d & affine) { { for (lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { @@ -74,14 +75,14 @@ void transformMaps( for (auto & pt : pcd_map_ptr->points) { Eigen::Vector3d eigen_pt(pt.x, pt.y, pt.z); auto transformed_pt = affine * eigen_pt; - pt.x = transformed_pt.x(); - pt.y = transformed_pt.y(); - pt.z = transformed_pt.z(); + pt.x = static_cast(transformed_pt.x()); + pt.y = static_cast(transformed_pt.y()); + pt.z = static_cast(transformed_pt.z()); } } } -Eigen::Affine3d createAffineMatrixFromXYZRPY( +Eigen::Affine3d create_affine_matrix_from_xyzrpy( const double x, const double y, const double z, const double roll, const double pitch, const double yaw) { @@ -127,19 +128,19 @@ int main(int argc, char * argv[]) pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { return EXIT_FAILURE; } - if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { return EXIT_FAILURE; } - Eigen::Affine3d affine = createAffineMatrixFromXYZRPY(x, y, z, roll, pitch, yaw); + Eigen::Affine3d affine = create_affine_matrix_from_xyzrpy(x, y, z, roll, pitch, yaw); const auto mgrs_grid = node->declare_parameter("mgrs_grid", projector.getProjectedMGRSGrid()); std::cout << "using mgrs grid: " << mgrs_grid << std::endl; - transformMaps(pcd_map_ptr, llt_map_ptr, affine); + transform_maps(pcd_map_ptr, llt_map_ptr, affine); lanelet::write(llt_output_path, *llt_map_ptr, projector); pcl::io::savePCDFileBinary(pcd_output_path, *pcd_map_ptr); diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/autoware_cluster_merger/CMakeLists.txt similarity index 94% rename from perception/cluster_merger/CMakeLists.txt rename to perception/autoware_cluster_merger/CMakeLists.txt index 773cea6383f41..ce666be317d6c 100644 --- a/perception/cluster_merger/CMakeLists.txt +++ b/perception/autoware_cluster_merger/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(cluster_merger) +project(autoware_cluster_merger) # find dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/cluster_merger/README.md b/perception/autoware_cluster_merger/README.md similarity index 94% rename from perception/cluster_merger/README.md rename to perception/autoware_cluster_merger/README.md index 6b719a1e5a480..771e6eb0342f0 100644 --- a/perception/cluster_merger/README.md +++ b/perception/autoware_cluster_merger/README.md @@ -1,8 +1,8 @@ -# cluster merger +# autoware cluster merger ## Purpose -cluster_merger is a package for merging pointcloud clusters as detected objects with feature type. +autoware_cluster_merger is a package for merging pointcloud clusters as detected objects with feature type. ## Inner-working / Algorithms diff --git a/perception/cluster_merger/config/cluster_merger.param.yaml b/perception/autoware_cluster_merger/config/cluster_merger.param.yaml similarity index 100% rename from perception/cluster_merger/config/cluster_merger.param.yaml rename to perception/autoware_cluster_merger/config/cluster_merger.param.yaml diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/autoware_cluster_merger/launch/cluster_merger.launch.xml similarity index 67% rename from perception/cluster_merger/launch/cluster_merger.launch.xml rename to perception/autoware_cluster_merger/launch/cluster_merger.launch.xml index f0f90efe051a0..40bdf643fbecf 100644 --- a/perception/cluster_merger/launch/cluster_merger.launch.xml +++ b/perception/autoware_cluster_merger/launch/cluster_merger.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/perception/cluster_merger/package.xml b/perception/autoware_cluster_merger/package.xml similarity index 96% rename from perception/cluster_merger/package.xml rename to perception/autoware_cluster_merger/package.xml index 17455dc27b6cc..41bcdb27b1387 100644 --- a/perception/cluster_merger/package.xml +++ b/perception/autoware_cluster_merger/package.xml @@ -1,7 +1,7 @@ - cluster_merger + autoware_cluster_merger 0.1.0 The ROS 2 cluster merger package Yukihiro Saito diff --git a/perception/cluster_merger/src/cluster_merger_node.cpp b/perception/autoware_cluster_merger/src/cluster_merger_node.cpp similarity index 100% rename from perception/cluster_merger/src/cluster_merger_node.cpp rename to perception/autoware_cluster_merger/src/cluster_merger_node.cpp diff --git a/perception/cluster_merger/src/cluster_merger_node.hpp b/perception/autoware_cluster_merger/src/cluster_merger_node.hpp similarity index 100% rename from perception/cluster_merger/src/cluster_merger_node.hpp rename to perception/autoware_cluster_merger/src/cluster_merger_node.hpp diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/autoware_detection_by_tracker/CMakeLists.txt similarity index 82% rename from perception/detection_by_tracker/CMakeLists.txt rename to perception/autoware_detection_by_tracker/CMakeLists.txt index 112187551d924..695e284459cc1 100644 --- a/perception/detection_by_tracker/CMakeLists.txt +++ b/perception/autoware_detection_by_tracker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(detection_by_tracker) +project(autoware_detection_by_tracker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -17,21 +17,15 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) include_directories( - include SYSTEM ${EIGEN3_INCLUDE_DIRS} ${PCL_COMMON_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) -# Generate exe file -set(DETECTION_BY_TRACKER_SRC - src/detection_by_tracker_node.cpp - src/utils/utils.cpp -) - ament_auto_add_library(${PROJECT_NAME} SHARED - ${DETECTION_BY_TRACKER_SRC} + src/detection_by_tracker_node.cpp + src/tracker/tracker_handler.cpp ) target_link_libraries(${PROJECT_NAME} @@ -43,9 +37,7 @@ rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::detection_by_tracker::DetectionByTracker" EXECUTABLE detection_by_tracker_node ) - -ament_auto_package( - INSTALL_TO_SHARE +ament_auto_package(INSTALL_TO_SHARE launch config ) diff --git a/perception/detection_by_tracker/README.md b/perception/autoware_detection_by_tracker/README.md similarity index 92% rename from perception/detection_by_tracker/README.md rename to perception/autoware_detection_by_tracker/README.md index 9c54c4fa7f4af..44cb287872f39 100644 --- a/perception/detection_by_tracker/README.md +++ b/perception/autoware_detection_by_tracker/README.md @@ -1,16 +1,16 @@ -# detection_by_tracker +# autoware_detection_by_tracker ## Purpose This package feeds back the tracked objects to the detection module to keep it stable and keep detecting objects. ![purpose](image/purpose.svg) -The detection by tracker takes as input an unknown object containing a cluster of points and a tracker. +The autoware detection by tracker takes as input an unknown object containing a cluster of points and a tracker. The unknown object is optimized to fit the size of the tracker so that it can continue to be detected. ## Inner-workings / Algorithms -The detection by tracker receives an unknown object containing a point cloud and a tracker, where the unknown object is mainly shape-fitted using euclidean clustering. +The autoware detection by tracker receives an unknown object containing a point cloud and a tracker, where the unknown object is mainly shape-fitted using euclidean clustering. Shape fitting using euclidean clustering and other methods has a problem called under segmentation and over segmentation. [![segmentation_fail](image/segmentation_fail.png)](https://www.researchgate.net/figure/Examples-of-an-undersegmentation-error-top-and-an-oversegmentation-error-bottom-Each_fig1_304533062) diff --git a/perception/detection_by_tracker/config/detection_by_tracker.param.yaml b/perception/autoware_detection_by_tracker/config/detection_by_tracker.param.yaml similarity index 100% rename from perception/detection_by_tracker/config/detection_by_tracker.param.yaml rename to perception/autoware_detection_by_tracker/config/detection_by_tracker.param.yaml diff --git a/perception/detection_by_tracker/image/purpose.svg b/perception/autoware_detection_by_tracker/image/purpose.svg similarity index 100% rename from perception/detection_by_tracker/image/purpose.svg rename to perception/autoware_detection_by_tracker/image/purpose.svg diff --git a/perception/detection_by_tracker/image/segmentation_fail.png b/perception/autoware_detection_by_tracker/image/segmentation_fail.png similarity index 100% rename from perception/detection_by_tracker/image/segmentation_fail.png rename to perception/autoware_detection_by_tracker/image/segmentation_fail.png diff --git a/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml b/perception/autoware_detection_by_tracker/launch/detection_by_tracker.launch.xml similarity index 75% rename from perception/detection_by_tracker/launch/detection_by_tracker.launch.xml rename to perception/autoware_detection_by_tracker/launch/detection_by_tracker.launch.xml index 3fba5cb58f8f3..932f08862dca2 100644 --- a/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml +++ b/perception/autoware_detection_by_tracker/launch/detection_by_tracker.launch.xml @@ -3,8 +3,8 @@ - - + + diff --git a/perception/detection_by_tracker/package.xml b/perception/autoware_detection_by_tracker/package.xml similarity index 90% rename from perception/detection_by_tracker/package.xml rename to perception/autoware_detection_by_tracker/package.xml index 50bd876ce0605..837dfae1e2ef9 100644 --- a/perception/detection_by_tracker/package.xml +++ b/perception/autoware_detection_by_tracker/package.xml @@ -1,9 +1,9 @@ - detection_by_tracker + autoware_detection_by_tracker 1.0.0 - The ROS 2 detection_by_tracker package + The ROS 2 autoware_detection_by_tracker package Yukihiro Saito Yoshi Ri Taekjin Lee diff --git a/perception/detection_by_tracker/schema/detection_by_tracker.schema.json b/perception/autoware_detection_by_tracker/schema/detection_by_tracker.schema.json similarity index 100% rename from perception/detection_by_tracker/schema/detection_by_tracker.schema.json rename to perception/autoware_detection_by_tracker/schema/detection_by_tracker.schema.json diff --git a/perception/detection_by_tracker/src/debugger/debugger.hpp b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp similarity index 95% rename from perception/detection_by_tracker/src/debugger/debugger.hpp rename to perception/autoware_detection_by_tracker/src/debugger/debugger.hpp index 242056c3ca316..bda42aa5f0b55 100644 --- a/perception/detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp @@ -17,10 +17,6 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" -#include "shape_estimation/shape_estimator.hpp" #include @@ -46,7 +42,6 @@ #include #include #include - namespace autoware::detection_by_tracker { class Debugger diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp similarity index 84% rename from perception/detection_by_tracker/src/detection_by_tracker_node.cpp rename to perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp index c41d6002d9266..4c65500d96aaa 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.cpp @@ -56,24 +56,26 @@ autoware_perception_msgs::msg::Shape extendShape( return output; } -boost::optional getReferenceYawInfo(const uint8_t label, const float yaw) +boost::optional getReferenceYawInfo( + const uint8_t label, const float yaw) { const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; + return autoware::shape_estimation::ReferenceYawInfo{yaw, autoware::universe_utils::deg2rad(30)}; } else { return boost::none; } } -boost::optional getReferenceShapeSizeInfo( +boost::optional getReferenceShapeSizeInfo( const uint8_t label, const autoware_perception_msgs::msg::Shape & shape) { const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || Label::TRAILER == label; if (is_vehicle) { - return ReferenceShapeSizeInfo{shape, ReferenceShapeSizeInfo::Mode::Min}; + return autoware::shape_estimation::ReferenceShapeSizeInfo{ + shape, autoware::shape_estimation::ReferenceShapeSizeInfo::Mode::Min}; } else { return boost::none; } @@ -83,70 +85,6 @@ boost::optional getReferenceShapeSizeInfo( namespace autoware::detection_by_tracker { -void TrackerHandler::onTrackedObjects( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) -{ - constexpr size_t max_buffer_size = 10; - - // Add tracked objects to buffer - objects_buffer_.push_front(*msg); - - // Remove old data - while (max_buffer_size < objects_buffer_.size()) { - objects_buffer_.pop_back(); - } -} - -bool TrackerHandler::estimateTrackedObjects( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output) -{ - if (objects_buffer_.empty()) { - return false; - } - - // Get the objects closest to the target time. - const auto target_objects_iter = std::min_element( - objects_buffer_.cbegin(), objects_buffer_.cend(), - [&time]( - autoware_perception_msgs::msg::TrackedObjects first, - autoware_perception_msgs::msg::TrackedObjects second) { - return std::fabs((time - first.header.stamp).seconds()) < - std::fabs((time - second.header.stamp).seconds()); - }); - - // Estimate the pose of the object at the target time - const auto dt = time - target_objects_iter->header.stamp; - output.header.frame_id = target_objects_iter->header.frame_id; - output.header.stamp = time; - for (const auto & object : target_objects_iter->objects) { - const auto & pose_with_covariance = object.kinematics.pose_with_covariance; - const auto & x = pose_with_covariance.pose.position.x; - const auto & y = pose_with_covariance.pose.position.y; - const auto & z = pose_with_covariance.pose.position.z; - const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation); - const auto & twist = object.kinematics.twist_with_covariance.twist; - const float & vx = twist.linear.x; - const float & wz = twist.angular.z; - - // build output - autoware_perception_msgs::msg::TrackedObject estimated_object; - estimated_object.object_id = object.object_id; - estimated_object.existence_probability = object.existence_probability; - estimated_object.classification = object.classification; - estimated_object.shape = object.shape; - estimated_object.kinematics.pose_with_covariance.pose.position.x = - x + vx * std::cos(yaw) * dt.seconds(); - estimated_object.kinematics.pose_with_covariance.pose.position.y = - y + vx * std::sin(yaw) * dt.seconds(); - estimated_object.kinematics.pose_with_covariance.pose.position.z = z; - const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); - estimated_object.kinematics.pose_with_covariance.pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_hat); - output.objects.push_back(estimated_object); - } - return true; -} - DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("detection_by_tracker", node_options), tf_buffer_(this->get_clock()), @@ -176,8 +114,8 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) // set maximum search setting for merger/divider setMaxSearchRange(); - shape_estimator_ = std::make_shared(true, true); - cluster_ = std::make_shared( + shape_estimator_ = std::make_shared(true, true); + cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); published_time_publisher_ = @@ -339,7 +277,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( const auto & label = target_object.classification.front().label; // initialize clustering parameters - euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( + autoware::euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( false, 4, 10000, initial_cluster_range, initial_voxel_size, 0); // convert to pcl @@ -468,7 +406,6 @@ void DetectionByTracker::mergeOverSegmentedObjects( out_objects.feature_objects.push_back(feature_object); } } - } // namespace autoware::detection_by_tracker #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.hpp similarity index 81% rename from perception/detection_by_tracker/src/detection_by_tracker_node.hpp rename to perception/autoware_detection_by_tracker/src/detection_by_tracker_node.hpp index 4e58d53fa71aa..f0ed51dd1938e 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/autoware_detection_by_tracker/src/detection_by_tracker_node.hpp @@ -15,12 +15,13 @@ #ifndef DETECTION_BY_TRACKER_NODE_HPP_ #define DETECTION_BY_TRACKER_NODE_HPP_ +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "debugger/debugger.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "tracker/tracker_handler.hpp" #include "utils/utils.hpp" #include @@ -52,19 +53,6 @@ namespace autoware::detection_by_tracker { -class TrackerHandler -{ -private: - std::deque objects_buffer_; - -public: - TrackerHandler() = default; - void onTrackedObjects( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg); - bool estimateTrackedObjects( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output); -}; - class DetectionByTracker : public rclcpp::Node { public: @@ -80,13 +68,13 @@ class DetectionByTracker : public rclcpp::Node tf2_ros::TransformListener tf_listener_; TrackerHandler tracker_handler_; - std::shared_ptr shape_estimator_; - std::shared_ptr cluster_; + std::shared_ptr shape_estimator_; + std::shared_ptr cluster_; std::shared_ptr debugger_; std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; - utils::TrackerIgnoreLabel tracker_ignore_; + detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; std::unique_ptr published_time_publisher_; @@ -97,7 +85,7 @@ class DetectionByTracker : public rclcpp::Node void divideUnderSegmentedObjects( const autoware_perception_msgs::msg::DetectedObjects & tracked_objects, - const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_objects, + const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_cluster_objects, autoware_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects); @@ -108,11 +96,10 @@ class DetectionByTracker : public rclcpp::Node void mergeOverSegmentedObjects( const autoware_perception_msgs::msg::DetectedObjects & tracked_objects, - const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_objects, + const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_cluster_objects, autoware_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects); }; - } // namespace autoware::detection_by_tracker #endif // DETECTION_BY_TRACKER_NODE_HPP_ diff --git a/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp new file mode 100644 index 0000000000000..a099c8b1ba55d --- /dev/null +++ b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp @@ -0,0 +1,88 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tracker_handler.hpp" + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" + +#include + +namespace autoware::detection_by_tracker +{ + +void TrackerHandler::onTrackedObjects( + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) +{ + constexpr size_t max_buffer_size = 10; + + // Add tracked objects to buffer + objects_buffer_.push_front(*msg); + + // Remove old data + while (max_buffer_size < objects_buffer_.size()) { + objects_buffer_.pop_back(); + } +} + +bool TrackerHandler::estimateTrackedObjects( + const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output) +{ + if (objects_buffer_.empty()) { + return false; + } + + // Get the objects closest to the target time. + const auto target_objects_iter = std::min_element( + objects_buffer_.cbegin(), objects_buffer_.cend(), + [&time]( + autoware_perception_msgs::msg::TrackedObjects first, + autoware_perception_msgs::msg::TrackedObjects second) { + return std::fabs((time - first.header.stamp).seconds()) < + std::fabs((time - second.header.stamp).seconds()); + }); + + // Estimate the pose of the object at the target time + const auto dt = time - target_objects_iter->header.stamp; + output.header.frame_id = target_objects_iter->header.frame_id; + output.header.stamp = time; + for (const auto & object : target_objects_iter->objects) { + const auto & pose_with_covariance = object.kinematics.pose_with_covariance; + const auto & x = pose_with_covariance.pose.position.x; + const auto & y = pose_with_covariance.pose.position.y; + const auto & z = pose_with_covariance.pose.position.z; + const float yaw = tf2::getYaw(pose_with_covariance.pose.orientation); + const auto & twist = object.kinematics.twist_with_covariance.twist; + const float & vx = twist.linear.x; + const float & wz = twist.angular.z; + + // build output + autoware_perception_msgs::msg::TrackedObject estimated_object; + estimated_object.object_id = object.object_id; + estimated_object.existence_probability = object.existence_probability; + estimated_object.classification = object.classification; + estimated_object.shape = object.shape; + estimated_object.kinematics.pose_with_covariance.pose.position.x = + x + vx * std::cos(yaw) * dt.seconds(); + estimated_object.kinematics.pose_with_covariance.pose.position.y = + y + vx * std::sin(yaw) * dt.seconds(); + estimated_object.kinematics.pose_with_covariance.pose.position.z = z; + const float yaw_hat = autoware::universe_utils::normalizeRadian(yaw + wz * dt.seconds()); + estimated_object.kinematics.pose_with_covariance.pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(yaw_hat); + output.objects.push_back(estimated_object); + } + return true; +} +} // namespace autoware::detection_by_tracker diff --git a/perception/detection_by_tracker/src/utils/utils.cpp b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.hpp similarity index 50% rename from perception/detection_by_tracker/src/utils/utils.cpp rename to perception/autoware_detection_by_tracker/src/tracker/tracker_handler.hpp index b269d703be7d4..13aa00aaf0c4c 100644 --- a/perception/detection_by_tracker/src/utils/utils.cpp +++ b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,22 +12,31 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils.hpp" +#ifndef TRACKER__TRACKER_HANDLER_HPP_ +#define TRACKER__TRACKER_HANDLER_HPP_ -#include +#include + +#include "autoware_perception_msgs/msg/tracked_objects.hpp" + +#include namespace autoware::detection_by_tracker { -namespace utils -{ -using Label = autoware_perception_msgs::msg::ObjectClassification; -bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const +class TrackerHandler { - return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || - (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || - (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || - (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); -} -} // namespace utils +private: + std::deque objects_buffer_; + +public: + TrackerHandler() = default; + void onTrackedObjects( + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg); + bool estimateTrackedObjects( + const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & output); +}; + } // namespace autoware::detection_by_tracker + +#endif // TRACKER__TRACKER_HANDLER_HPP_ diff --git a/perception/detection_by_tracker/src/utils/utils.hpp b/perception/autoware_detection_by_tracker/src/utils/utils.hpp similarity index 64% rename from perception/detection_by_tracker/src/utils/utils.hpp rename to perception/autoware_detection_by_tracker/src/utils/utils.hpp index b7041124fa931..cc29164e4aad9 100644 --- a/perception/detection_by_tracker/src/utils/utils.hpp +++ b/perception/autoware_detection_by_tracker/src/utils/utils.hpp @@ -15,12 +15,16 @@ #ifndef UTILS__UTILS_HPP_ #define UTILS__UTILS_HPP_ +#include "autoware_perception_msgs/msg/object_classification.hpp" + #include namespace autoware::detection_by_tracker { namespace utils { +using Label = autoware_perception_msgs::msg::ObjectClassification; + struct TrackerIgnoreLabel { bool UNKNOWN; @@ -31,8 +35,15 @@ struct TrackerIgnoreLabel bool MOTORCYCLE; bool BICYCLE; bool PEDESTRIAN; - bool isIgnore(const uint8_t label) const; -}; // struct TrackerIgnoreLabel + bool isIgnore(const uint8_t label) const + { + return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || + (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || + (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || + (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); + } +}; + } // namespace utils } // namespace autoware::detection_by_tracker diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 2b8358eb52e4e..478e8d6f72dd3 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -1017,7 +1017,6 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt std::unordered_set predicted_crosswalk_users_ids; for (const auto & object : in_objects->objects) { - std::string object_id = autoware::universe_utils::toHexString(object.object_id); TrackedObject transformed_object = object; // transform object frame if it's based on map frame @@ -1309,10 +1308,14 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { - const lanelet::ConstLineStrings3d & all_fences = - lanelet::utils::query::getAllFences(lanelet_map_ptr_); - for (const auto & fence_line : all_fences) { - if (doesPathCrossFence(predicted_path, fence_line)) { + lanelet::BasicLineString2d predicted_path_ls; + for (const auto & p : predicted_path.path) + predicted_path_ls.emplace_back(p.position.x, p.position.y); + const auto candidates = + lanelet_map_ptr_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls)); + for (const auto & candidate : candidates) { + const std::string type = candidate.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "fence" && doesPathCrossFence(predicted_path, candidate)) { return true; } } diff --git a/perception/object_range_splitter/CMakeLists.txt b/perception/autoware_object_range_splitter/CMakeLists.txt similarity index 90% rename from perception/object_range_splitter/CMakeLists.txt rename to perception/autoware_object_range_splitter/CMakeLists.txt index 1da71a31b26d0..2c2d8e4108578 100644 --- a/perception/object_range_splitter/CMakeLists.txt +++ b/perception/autoware_object_range_splitter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(object_range_splitter) +project(autoware_object_range_splitter) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/object_range_splitter/README.md b/perception/autoware_object_range_splitter/README.md similarity index 94% rename from perception/object_range_splitter/README.md rename to perception/autoware_object_range_splitter/README.md index 39375c3139b49..3cdfe36528094 100644 --- a/perception/object_range_splitter/README.md +++ b/perception/autoware_object_range_splitter/README.md @@ -1,8 +1,8 @@ -# object_range_splitter +# `autoware_object_range_splitter` ## Purpose -object_range_splitter is a package to divide detected objects into two messages by the distance from the origin. +autoware_object_range_splitter is a package to divide detected objects into two messages by the distance from the origin. ## Inner-workings / Algorithms diff --git a/perception/object_range_splitter/config/object_range_splitter.param.yaml b/perception/autoware_object_range_splitter/config/object_range_splitter.param.yaml similarity index 100% rename from perception/object_range_splitter/config/object_range_splitter.param.yaml rename to perception/autoware_object_range_splitter/config/object_range_splitter.param.yaml diff --git a/perception/object_range_splitter/launch/object_range_splitter.launch.xml b/perception/autoware_object_range_splitter/launch/object_range_splitter.launch.xml similarity index 66% rename from perception/object_range_splitter/launch/object_range_splitter.launch.xml rename to perception/autoware_object_range_splitter/launch/object_range_splitter.launch.xml index 3f2f3c6ba24c6..77ac04681ac7f 100644 --- a/perception/object_range_splitter/launch/object_range_splitter.launch.xml +++ b/perception/autoware_object_range_splitter/launch/object_range_splitter.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/perception/object_range_splitter/package.xml b/perception/autoware_object_range_splitter/package.xml similarity index 86% rename from perception/object_range_splitter/package.xml rename to perception/autoware_object_range_splitter/package.xml index 7fc9245ee894f..1f678f72b7942 100644 --- a/perception/object_range_splitter/package.xml +++ b/perception/autoware_object_range_splitter/package.xml @@ -1,9 +1,9 @@ - object_range_splitter + autoware_object_range_splitter 0.1.0 - The object_range_splitter package + The autoware_object_range_splitter package Yukihiro Saito Yoshi Ri Apache License 2.0 diff --git a/perception/object_range_splitter/schema/object_range_splitter.schema.json b/perception/autoware_object_range_splitter/schema/object_range_splitter.schema.json similarity index 100% rename from perception/object_range_splitter/schema/object_range_splitter.schema.json rename to perception/autoware_object_range_splitter/schema/object_range_splitter.schema.json diff --git a/perception/object_range_splitter/src/object_range_splitter_node.cpp b/perception/autoware_object_range_splitter/src/object_range_splitter_node.cpp similarity index 100% rename from perception/object_range_splitter/src/object_range_splitter_node.cpp rename to perception/autoware_object_range_splitter/src/object_range_splitter_node.cpp diff --git a/perception/object_range_splitter/src/object_range_splitter_node.hpp b/perception/autoware_object_range_splitter/src/object_range_splitter_node.hpp similarity index 100% rename from perception/object_range_splitter/src/object_range_splitter_node.hpp rename to perception/autoware_object_range_splitter/src/object_range_splitter_node.hpp diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/autoware_object_velocity_splitter/CMakeLists.txt similarity index 93% rename from perception/object_velocity_splitter/CMakeLists.txt rename to perception/autoware_object_velocity_splitter/CMakeLists.txt index afad278e383d8..ec6b57083e058 100644 --- a/perception/object_velocity_splitter/CMakeLists.txt +++ b/perception/autoware_object_velocity_splitter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(object_velocity_splitter) +project(autoware_object_velocity_splitter) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/object_velocity_splitter/README.md b/perception/autoware_object_velocity_splitter/README.md similarity index 95% rename from perception/object_velocity_splitter/README.md rename to perception/autoware_object_velocity_splitter/README.md index 659ae5845f46a..ac57d5d7f6353 100644 --- a/perception/object_velocity_splitter/README.md +++ b/perception/autoware_object_velocity_splitter/README.md @@ -1,4 +1,4 @@ -# object_velocity_splitter +# autoware_object_velocity_splitter This package contains a object filter module for [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.idl). This package can split DetectedObjects into two messages by object's speed. diff --git a/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml b/perception/autoware_object_velocity_splitter/config/object_velocity_splitter.param.yaml similarity index 100% rename from perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml rename to perception/autoware_object_velocity_splitter/config/object_velocity_splitter.param.yaml diff --git a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml b/perception/autoware_object_velocity_splitter/launch/object_velocity_splitter.launch.xml similarity index 68% rename from perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml rename to perception/autoware_object_velocity_splitter/launch/object_velocity_splitter.launch.xml index 8ab654d4907a4..f3a359d0ca246 100644 --- a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml +++ b/perception/autoware_object_velocity_splitter/launch/object_velocity_splitter.launch.xml @@ -5,10 +5,10 @@ - + - + diff --git a/perception/object_velocity_splitter/package.xml b/perception/autoware_object_velocity_splitter/package.xml similarity index 89% rename from perception/object_velocity_splitter/package.xml rename to perception/autoware_object_velocity_splitter/package.xml index 32fd5b2ff6337..eb233150a44d9 100644 --- a/perception/object_velocity_splitter/package.xml +++ b/perception/autoware_object_velocity_splitter/package.xml @@ -1,9 +1,9 @@ - object_velocity_splitter + autoware_object_velocity_splitter 0.1.0 - object_velocity_splitter + autoware_object_velocity_splitter Sathshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp similarity index 100% rename from perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp rename to perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.hpp similarity index 100% rename from perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp rename to perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.hpp diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/autoware_radar_crossing_objects_noise_filter/CMakeLists.txt similarity index 94% rename from perception/radar_crossing_objects_noise_filter/CMakeLists.txt rename to perception/autoware_radar_crossing_objects_noise_filter/CMakeLists.txt index 6414739413d5c..aebd29f72b360 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/autoware_radar_crossing_objects_noise_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_crossing_objects_noise_filter) +project(autoware_radar_crossing_objects_noise_filter) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/radar_crossing_objects_noise_filter/README.md b/perception/autoware_radar_crossing_objects_noise_filter/README.md similarity index 98% rename from perception/radar_crossing_objects_noise_filter/README.md rename to perception/autoware_radar_crossing_objects_noise_filter/README.md index 96a624a1d4872..846da0a0c11d1 100644 --- a/perception/radar_crossing_objects_noise_filter/README.md +++ b/perception/autoware_radar_crossing_objects_noise_filter/README.md @@ -1,4 +1,4 @@ -# radar_crossing_objects_noise_filter +# autoware_radar_crossing_objects_noise_filter This package contains a radar noise filter module for [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.idl). This package can filter the noise objects which cross to the ego vehicle. diff --git a/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml b/perception/autoware_radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml similarity index 100% rename from perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml rename to perception/autoware_radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml diff --git a/perception/radar_crossing_objects_noise_filter/docs/radar_crossing_objects_noise_filter.drawio.svg b/perception/autoware_radar_crossing_objects_noise_filter/docs/radar_crossing_objects_noise_filter.drawio.svg similarity index 100% rename from perception/radar_crossing_objects_noise_filter/docs/radar_crossing_objects_noise_filter.drawio.svg rename to perception/autoware_radar_crossing_objects_noise_filter/docs/radar_crossing_objects_noise_filter.drawio.svg diff --git a/perception/radar_crossing_objects_noise_filter/docs/turning_around.png b/perception/autoware_radar_crossing_objects_noise_filter/docs/turning_around.png similarity index 100% rename from perception/radar_crossing_objects_noise_filter/docs/turning_around.png rename to perception/autoware_radar_crossing_objects_noise_filter/docs/turning_around.png diff --git a/perception/radar_crossing_objects_noise_filter/docs/vertical_velocity_objects.png b/perception/autoware_radar_crossing_objects_noise_filter/docs/vertical_velocity_objects.png similarity index 100% rename from perception/radar_crossing_objects_noise_filter/docs/vertical_velocity_objects.png rename to perception/autoware_radar_crossing_objects_noise_filter/docs/vertical_velocity_objects.png diff --git a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml b/perception/autoware_radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml similarity index 63% rename from perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml rename to perception/autoware_radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml index 25856e12c3ba5..54cafd5086642 100644 --- a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml +++ b/perception/autoware_radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml @@ -5,10 +5,10 @@ - + - + diff --git a/perception/radar_crossing_objects_noise_filter/package.xml b/perception/autoware_radar_crossing_objects_noise_filter/package.xml similarity index 88% rename from perception/radar_crossing_objects_noise_filter/package.xml rename to perception/autoware_radar_crossing_objects_noise_filter/package.xml index d0245dfe62a40..6476d66eef4f8 100644 --- a/perception/radar_crossing_objects_noise_filter/package.xml +++ b/perception/autoware_radar_crossing_objects_noise_filter/package.xml @@ -1,9 +1,9 @@ - radar_crossing_objects_noise_filter + autoware_radar_crossing_objects_noise_filter 0.1.0 - radar_crossing_objects_noise_filter + autoware_radar_crossing_objects_noise_filter Sathshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp b/perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp similarity index 100% rename from perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp rename to perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp b/perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp similarity index 100% rename from perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp rename to perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp b/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp similarity index 100% rename from perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp rename to perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp similarity index 100% rename from perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp rename to perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp diff --git a/perception/radar_fusion_to_detected_object/CMakeLists.txt b/perception/autoware_radar_fusion_to_detected_object/CMakeLists.txt similarity index 93% rename from perception/radar_fusion_to_detected_object/CMakeLists.txt rename to perception/autoware_radar_fusion_to_detected_object/CMakeLists.txt index 76c27d3958841..aab25d6f54133 100644 --- a/perception/radar_fusion_to_detected_object/CMakeLists.txt +++ b/perception/autoware_radar_fusion_to_detected_object/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_fusion_to_detected_object) +project(autoware_radar_fusion_to_detected_object) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/autoware_radar_fusion_to_detected_object/README.md similarity index 96% rename from perception/radar_fusion_to_detected_object/README.md rename to perception/autoware_radar_fusion_to_detected_object/README.md index 831dbd24e4360..c57b710b09619 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/autoware_radar_fusion_to_detected_object/README.md @@ -1,4 +1,4 @@ -# radar_fusion_to_detected_object +# `autoware_radar_fusion_to_detected_object` This package contains a sensor fusion module for radar-detected objects and 3D detected objects. @@ -94,7 +94,7 @@ If the probability of an output object is lower than this parameter, and the out This parameter is the flag to use probability compensation. If this parameter is true, compensate probability of objects to threshold probability. -## Interface for radar_object_fusion_to_detected_object +## Interface for `autoware_radar_object_fusion_to_detected_object` Sensor fusion with radar objects and a detected object. @@ -105,7 +105,7 @@ Sensor fusion with radar objects and a detected object. ### How to launch ```sh -ros2 launch radar_fusion_to_detected_object radar_object_to_detected_object.launch.xml +ros2 launch autoware_radar_fusion_to_detected_object radar_object_to_detected_object.launch.xml ``` ### Input diff --git a/perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml b/perception/autoware_radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml similarity index 100% rename from perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml rename to perception/autoware_radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml diff --git a/perception/radar_fusion_to_detected_object/docs/algorithm.md b/perception/autoware_radar_fusion_to_detected_object/docs/algorithm.md similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/algorithm.md rename to perception/autoware_radar_fusion_to_detected_object/docs/algorithm.md diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml b/perception/autoware_radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml similarity index 60% rename from perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml rename to perception/autoware_radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml index e5505ad87d9ec..d074c66464f72 100644 --- a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml +++ b/perception/autoware_radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml @@ -5,10 +5,10 @@ - + - + diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/autoware_radar_fusion_to_detected_object/package.xml similarity index 89% rename from perception/radar_fusion_to_detected_object/package.xml rename to perception/autoware_radar_fusion_to_detected_object/package.xml index 2094ff7770555..94a0c55574f60 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/autoware_radar_fusion_to_detected_object/package.xml @@ -1,9 +1,9 @@ - radar_fusion_to_detected_object + autoware_radar_fusion_to_detected_object 0.0.0 - radar_fusion_to_detected_object + autoware_radar_fusion_to_detected_object Satoshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json b/perception/autoware_radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json similarity index 100% rename from perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json rename to perception/autoware_radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json diff --git a/perception/radar_fusion_to_detected_object/src/include/node.hpp b/perception/autoware_radar_fusion_to_detected_object/src/include/node.hpp similarity index 100% rename from perception/radar_fusion_to_detected_object/src/include/node.hpp rename to perception/autoware_radar_fusion_to_detected_object/src/include/node.hpp diff --git a/perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp b/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp similarity index 100% rename from perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp rename to perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp diff --git a/perception/radar_fusion_to_detected_object/src/node.cpp b/perception/autoware_radar_fusion_to_detected_object/src/node.cpp similarity index 100% rename from perception/radar_fusion_to_detected_object/src/node.cpp rename to perception/autoware_radar_fusion_to_detected_object/src/node.cpp diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp similarity index 99% rename from perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp rename to perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 0c90a468f2ebd..0fb87c0f88ba0 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -95,6 +95,8 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update( for (auto & split_object : split_objects) { // set radars within objects std::shared_ptr> radars_within_split_object; + + // cppcheck-suppress knownConditionTrueFalse if (split_objects.size() == 1) { // If object is not split, radar data within object is same radars_within_split_object = radars_within_object; diff --git a/perception/radar_object_clustering/CMakeLists.txt b/perception/autoware_radar_object_clustering/CMakeLists.txt similarity index 93% rename from perception/radar_object_clustering/CMakeLists.txt rename to perception/autoware_radar_object_clustering/CMakeLists.txt index 9a54bbb0dae43..43f6a3f73ebc0 100644 --- a/perception/radar_object_clustering/CMakeLists.txt +++ b/perception/autoware_radar_object_clustering/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_object_clustering) +project(autoware_radar_object_clustering) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/radar_object_clustering/README.md b/perception/autoware_radar_object_clustering/README.md similarity index 99% rename from perception/radar_object_clustering/README.md rename to perception/autoware_radar_object_clustering/README.md index 8f936ce61a1f3..5fbb4df81a115 100644 --- a/perception/radar_object_clustering/README.md +++ b/perception/autoware_radar_object_clustering/README.md @@ -1,4 +1,4 @@ -# radar_object_clustering +# `autoware_radar_object_clustering` This package contains a radar object clustering for [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.idl) input. diff --git a/perception/radar_object_clustering/config/radar_object_clustering.param.yaml b/perception/autoware_radar_object_clustering/config/radar_object_clustering.param.yaml similarity index 100% rename from perception/radar_object_clustering/config/radar_object_clustering.param.yaml rename to perception/autoware_radar_object_clustering/config/radar_object_clustering.param.yaml diff --git a/perception/radar_object_clustering/docs/clustering.drawio.svg b/perception/autoware_radar_object_clustering/docs/clustering.drawio.svg similarity index 100% rename from perception/radar_object_clustering/docs/clustering.drawio.svg rename to perception/autoware_radar_object_clustering/docs/clustering.drawio.svg diff --git a/perception/radar_object_clustering/docs/radar_clustering.drawio.svg b/perception/autoware_radar_object_clustering/docs/radar_clustering.drawio.svg similarity index 100% rename from perception/radar_object_clustering/docs/radar_clustering.drawio.svg rename to perception/autoware_radar_object_clustering/docs/radar_clustering.drawio.svg diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/autoware_radar_object_clustering/launch/radar_object_clustering.launch.xml similarity index 55% rename from perception/radar_object_clustering/launch/radar_object_clustering.launch.xml rename to perception/autoware_radar_object_clustering/launch/radar_object_clustering.launch.xml index 1759c17a5ab6b..a701b9f45b480 100644 --- a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml +++ b/perception/autoware_radar_object_clustering/launch/radar_object_clustering.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/perception/radar_object_clustering/package.xml b/perception/autoware_radar_object_clustering/package.xml similarity index 91% rename from perception/radar_object_clustering/package.xml rename to perception/autoware_radar_object_clustering/package.xml index c7666d2c5701e..d9c3464e02b18 100644 --- a/perception/radar_object_clustering/package.xml +++ b/perception/autoware_radar_object_clustering/package.xml @@ -1,9 +1,9 @@ - radar_object_clustering + autoware_radar_object_clustering 0.1.0 - radar_object_clustering + autoware_radar_object_clustering Sathshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node.cpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp similarity index 100% rename from perception/radar_object_clustering/src/radar_object_clustering_node.cpp rename to perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node.hpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.hpp similarity index 100% rename from perception/radar_object_clustering/src/radar_object_clustering_node.hpp rename to perception/autoware_radar_object_clustering/src/radar_object_clustering_node.hpp diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/autoware_radar_object_tracker/CMakeLists.txt similarity index 97% rename from perception/radar_object_tracker/CMakeLists.txt rename to perception/autoware_radar_object_tracker/CMakeLists.txt index b732ef6cc863c..614dbfc6e1a30 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/autoware_radar_object_tracker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(radar_object_tracker) +project(autoware_radar_object_tracker) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/radar_object_tracker/README.md b/perception/autoware_radar_object_tracker/README.md similarity index 99% rename from perception/radar_object_tracker/README.md rename to perception/autoware_radar_object_tracker/README.md index 056a27d8c4ed0..f9ca194de2491 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/autoware_radar_object_tracker/README.md @@ -1,4 +1,4 @@ -# Radar Object Tracker +# `autoware_radar_object_tracker` ## Purpose diff --git a/perception/radar_object_tracker/config/data_association_matrix.param.yaml b/perception/autoware_radar_object_tracker/config/data_association_matrix.param.yaml similarity index 100% rename from perception/radar_object_tracker/config/data_association_matrix.param.yaml rename to perception/autoware_radar_object_tracker/config/data_association_matrix.param.yaml diff --git a/perception/radar_object_tracker/config/default_tracker.param.yaml b/perception/autoware_radar_object_tracker/config/default_tracker.param.yaml similarity index 100% rename from perception/radar_object_tracker/config/default_tracker.param.yaml rename to perception/autoware_radar_object_tracker/config/default_tracker.param.yaml diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/autoware_radar_object_tracker/config/radar_object_tracker.param.yaml similarity index 100% rename from perception/radar_object_tracker/config/radar_object_tracker.param.yaml rename to perception/autoware_radar_object_tracker/config/radar_object_tracker.param.yaml diff --git a/perception/radar_object_tracker/config/simulation_tracker.param.yaml b/perception/autoware_radar_object_tracker/config/simulation_tracker.param.yaml similarity index 100% rename from perception/radar_object_tracker/config/simulation_tracker.param.yaml rename to perception/autoware_radar_object_tracker/config/simulation_tracker.param.yaml diff --git a/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml b/perception/autoware_radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml similarity index 100% rename from perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml rename to perception/autoware_radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml diff --git a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml b/perception/autoware_radar_object_tracker/config/tracking/linear_motion_tracker.yaml similarity index 100% rename from perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml rename to perception/autoware_radar_object_tracker/config/tracking/linear_motion_tracker.yaml diff --git a/perception/radar_object_tracker/image/noise_filtering.drawio.svg b/perception/autoware_radar_object_tracker/image/noise_filtering.drawio.svg similarity index 100% rename from perception/radar_object_tracker/image/noise_filtering.drawio.svg rename to perception/autoware_radar_object_tracker/image/noise_filtering.drawio.svg diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/autoware_radar_object_tracker/launch/radar_object_tracker.launch.xml similarity index 63% rename from perception/radar_object_tracker/launch/radar_object_tracker.launch.xml rename to perception/autoware_radar_object_tracker/launch/radar_object_tracker.launch.xml index 313ef4b0f9fcd..16e96e527af94 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/autoware_radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -2,12 +2,12 @@ - - - + + + - + diff --git a/perception/radar_object_tracker/models.md b/perception/autoware_radar_object_tracker/models.md similarity index 100% rename from perception/radar_object_tracker/models.md rename to perception/autoware_radar_object_tracker/models.md diff --git a/perception/radar_object_tracker/package.xml b/perception/autoware_radar_object_tracker/package.xml similarity index 92% rename from perception/radar_object_tracker/package.xml rename to perception/autoware_radar_object_tracker/package.xml index 79ae0385d4b5e..50dd70e6cec67 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/autoware_radar_object_tracker/package.xml @@ -1,12 +1,11 @@ - radar_object_tracker + autoware_radar_object_tracker 0.0.0 Do tracking radar object Yoshi Ri Yukihiro Saito - Satoshi Tanaka Taekjin Lee Apache License 2.0 diff --git a/perception/radar_object_tracker/src/association/data_association.cpp b/perception/autoware_radar_object_tracker/src/association/data_association.cpp similarity index 100% rename from perception/radar_object_tracker/src/association/data_association.cpp rename to perception/autoware_radar_object_tracker/src/association/data_association.cpp diff --git a/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp b/perception/autoware_radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp similarity index 100% rename from perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp rename to perception/autoware_radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp diff --git a/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp b/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp similarity index 98% rename from perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp rename to perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp index 7877fe5451d8d..d18e4f140e2ed 100644 --- a/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp +++ b/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp @@ -328,12 +328,12 @@ void SSP::maximizeLinearAssignment( #ifndef NDEBUG // Check if the potentials are feasible potentials - for (int v = 0; v < n_nodes; ++v) { - for (auto it_incident_edge = adjacency_list.at(v).cbegin(); - it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + for (int w = 0; w < n_nodes; ++w) { + for (auto it_incident_edge = adjacency_list.at(w).cbegin(); + it_incident_edge != adjacency_list.at(w).cend(); ++it_incident_edge) { if (it_incident_edge->capacity > 0) { double reduced_cost = - it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + it_incident_edge->cost + potentials.at(w) - potentials.at(it_incident_edge->dst); assert(reduced_cost >= 0); } } diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node.cpp b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp similarity index 100% rename from perception/radar_object_tracker/src/radar_object_tracker_node.cpp rename to perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node.hpp b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.hpp similarity index 98% rename from perception/radar_object_tracker/src/radar_object_tracker_node.hpp rename to perception/autoware_radar_object_tracker/src/radar_object_tracker_node.hpp index aaba15bbad05d..224fa10e45c32 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node.hpp +++ b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.hpp @@ -80,7 +80,7 @@ class RadarObjectTrackerNode : public rclcpp::Node void onMeasurement( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); - void onMap(const LaneletMapBin::ConstSharedPtr map_msg); + void onMap(const LaneletMapBin::ConstSharedPtr msg); std::string world_frame_id_; // tracking frame std::string tracker_config_directory_; diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp similarity index 100% rename from perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp rename to perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp similarity index 100% rename from perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp rename to perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp diff --git a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/tracker_base.cpp similarity index 100% rename from perception/radar_object_tracker/src/tracker/model/tracker_base.cpp rename to perception/autoware_radar_object_tracker/src/tracker/model/tracker_base.cpp diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp similarity index 100% rename from perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp rename to perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp diff --git a/perception/radar_object_tracker/src/utils/utils.cpp b/perception/autoware_radar_object_tracker/src/utils/utils.cpp similarity index 100% rename from perception/radar_object_tracker/src/utils/utils.cpp rename to perception/autoware_radar_object_tracker/src/utils/utils.cpp diff --git a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp b/perception/autoware_radar_object_tracker/test/test_radar_object_tracker_utils.cpp similarity index 100% rename from perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp rename to perception/autoware_radar_object_tracker/test/test_radar_object_tracker_utils.cpp diff --git a/perception/raindrop_cluster_filter/CMakeLists.txt b/perception/autoware_raindrop_cluster_filter/CMakeLists.txt similarity index 94% rename from perception/raindrop_cluster_filter/CMakeLists.txt rename to perception/autoware_raindrop_cluster_filter/CMakeLists.txt index 17eef97cc2f52..cc58bd4a80c12 100644 --- a/perception/raindrop_cluster_filter/CMakeLists.txt +++ b/perception/autoware_raindrop_cluster_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(raindrop_cluster_filter) +project(autoware_raindrop_cluster_filter) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/perception/raindrop_cluster_filter/config/low_intensity_cluster_filter.param.yaml b/perception/autoware_raindrop_cluster_filter/config/low_intensity_cluster_filter.param.yaml similarity index 100% rename from perception/raindrop_cluster_filter/config/low_intensity_cluster_filter.param.yaml rename to perception/autoware_raindrop_cluster_filter/config/low_intensity_cluster_filter.param.yaml diff --git a/perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml b/perception/autoware_raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml similarity index 71% rename from perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml rename to perception/autoware_raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml index 9af4ad47055f9..36d8664579070 100644 --- a/perception/raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml +++ b/perception/autoware_raindrop_cluster_filter/launch/low_intensity_cluster_filter.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/perception/raindrop_cluster_filter/package.xml b/perception/autoware_raindrop_cluster_filter/package.xml similarity index 96% rename from perception/raindrop_cluster_filter/package.xml rename to perception/autoware_raindrop_cluster_filter/package.xml index a660e42dd91ce..eea7dda76d4aa 100644 --- a/perception/raindrop_cluster_filter/package.xml +++ b/perception/autoware_raindrop_cluster_filter/package.xml @@ -1,7 +1,7 @@ - raindrop_cluster_filter + autoware_raindrop_cluster_filter 0.1.0 The ROS 2 filter cluster package Yukihiro Saito diff --git a/perception/raindrop_cluster_filter/raindrop_cluster_filter.md b/perception/autoware_raindrop_cluster_filter/raindrop_cluster_filter.md similarity index 100% rename from perception/raindrop_cluster_filter/raindrop_cluster_filter.md rename to perception/autoware_raindrop_cluster_filter/raindrop_cluster_filter.md diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp similarity index 97% rename from perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp rename to perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp index 105fc98722a26..62b6b9f3b09b5 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp +++ b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp @@ -125,9 +125,9 @@ bool LowIntensityClusterFilter::isValidatedCluster(const sensor_msgs::msg::Point RCLCPP_WARN(get_logger(), "Invalid point cloud data. point_step is less than 16."); return true; } - for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); iter != iter.end(); - ++iter) { - mean_intensity += *iter; + for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); + iter != iter.end(); ++iter) { + mean_intensity += static_cast(*iter); } const size_t num_points = cluster.width * cluster.height; mean_intensity /= static_cast(num_points); diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp similarity index 94% rename from perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp rename to perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp index 8e21306218ab2..61e23860cd195 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp +++ b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.hpp @@ -15,9 +15,9 @@ #ifndef LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ #define LOW_INTENSITY_CLUSTER_FILTER_NODE_HPP_ +#include "autoware/detected_object_validation/utils/utils.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "detected_object_validation/utils/utils.hpp" #include #include @@ -64,7 +64,7 @@ class LowIntensityClusterFilter : public rclcpp::Node // Eigen::Vector4f max_boundary_transformed_; bool is_validation_range_transformed_ = false; const std::string base_link_frame_id_ = "base_link"; - utils::FilterTargetLabel filter_target_; + autoware::detected_object_validation::utils::FilterTargetLabel filter_target_; // debugger std::unique_ptr> stop_watch_ptr_{ diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index a76b0fc56e2a0..0a33711e946ef 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -21,23 +21,23 @@ include_directories( ${GRID_MAP_INCLUDE_DIR} ) -add_library(compare_map_segmentation SHARED - src/distance_based_compare_map_filter_nodelet.cpp - src/voxel_based_approximate_compare_map_filter_nodelet.cpp - src/voxel_based_compare_map_filter_nodelet.cpp - src/voxel_distance_based_compare_map_filter_nodelet.cpp - src/compare_elevation_map_filter_node.cpp - src/voxel_grid_map_loader.cpp +add_library(${PROJECT_NAME} SHARED + src/distance_based_compare_map_filter/node.cpp + src/voxel_based_approximate_compare_map_filter/node.cpp + src/voxel_based_compare_map_filter/node.cpp + src/voxel_distance_based_compare_map_filter/node.cpp + src/compare_elevation_map_filter/node.cpp + src/voxel_grid_map_loader/voxel_grid_map_loader.cpp ) -target_link_libraries(compare_map_segmentation +target_link_libraries(${PROJECT_NAME} pointcloud_preprocessor::pointcloud_preprocessor_filter_base ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) -ament_target_dependencies(compare_map_segmentation +ament_target_dependencies(${PROJECT_NAME} grid_map_pcl grid_map_ros pcl_conversions @@ -50,7 +50,7 @@ ament_target_dependencies(compare_map_segmentation ) if(OPENMP_FOUND) - set_target_properties(compare_map_segmentation PROPERTIES + set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${OpenMP_CXX_FLAGS} LINK_FLAGS ${OpenMP_CXX_FLAGS} ) @@ -58,32 +58,32 @@ endif() # ========== Compare Map Filter ========== # -- Distance Based Compare Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::DistanceBasedCompareMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent" EXECUTABLE distance_based_compare_map_filter_node) # -- Voxel Based Approximate Compare Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent" EXECUTABLE voxel_based_approximate_compare_map_filter_node) # -- Voxel Based Compare Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::VoxelBasedCompareMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent" EXECUTABLE voxel_based_compare_map_filter_node) # -- Voxel Distance Based Compare Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent" EXECUTABLE voxel_distance_based_compare_map_filter_node) # -- Compare Elevation Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::CompareElevationMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::CompareElevationMapFilterComponent" EXECUTABLE compare_elevation_map_filter_node) install( - TARGETS compare_map_segmentation + TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp similarity index 93% rename from perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp rename to perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp index 1eadeeb3bec05..e5a19245bc5b1 100644 --- a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp +++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/compare_elevation_map_filter_node.hpp" +#include "node.hpp" #include #include @@ -33,7 +33,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { CompareElevationMapFilterComponent::CompareElevationMapFilterComponent( const rclcpp::NodeOptions & options) @@ -94,7 +94,8 @@ void CompareElevationMapFilterComponent::filter( output.header.stamp = input->header.stamp; output.header.frame_id = output_frame; } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include -RCLCPP_COMPONENTS_REGISTER_NODE(compare_map_segmentation::CompareElevationMapFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::compare_map_segmentation::CompareElevationMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp similarity index 86% rename from perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp rename to perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp index 63aad08fe7611..ba2328d862951 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp +++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_ -#define COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_ +#ifndef COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_ +#define COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_ #include "pointcloud_preprocessor/filter.hpp" @@ -29,7 +29,7 @@ #include #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { class CompareElevationMapFilterComponent : public pointcloud_preprocessor::Filter { @@ -56,6 +56,6 @@ class CompareElevationMapFilterComponent : public pointcloud_preprocessor::Filte PCL_MAKE_ALIGNED_OPERATOR_NEW explicit CompareElevationMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation -#endif // COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_ +#endif // COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_ diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp similarity index 94% rename from perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp rename to perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp index 2962ba1e28a75..53183cd7dc3f0 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp" +#include "node.hpp" + +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -20,7 +23,7 @@ #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { void DistanceBasedStaticMapLoader::onMapCallback( @@ -176,7 +179,8 @@ void DistanceBasedCompareMapFilterComponent::filter( } } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include -RCLCPP_COMPONENTS_REGISTER_NODE(compare_map_segmentation::DistanceBasedCompareMapFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp similarity index 91% rename from perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp rename to perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp index 301dc75839f8e..115a73cf0d263 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ -#define COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ +#ifndef DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ +#define DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ +#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include "voxel_grid_map_loader.hpp" #include // for pcl::isFinite #include @@ -26,7 +26,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { typedef typename pcl::Filter::PointCloud PointCloud; @@ -114,8 +114,8 @@ class DistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::F PCL_MAKE_ALIGNED_OPERATOR_NEW explicit DistanceBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation // clang-format off -#endif // COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#endif // DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT // clang-format on diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp similarity index 94% rename from perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp rename to perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index e14ae7d55a1d4..8bc22f8c31184 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp" +#include "node.hpp" + +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -20,7 +23,7 @@ #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { bool VoxelBasedApproximateStaticMapLoader::is_close_to_map( @@ -145,8 +148,8 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( } } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include RCLCPP_COMPONENTS_REGISTER_NODE( - compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent) + autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp similarity index 85% rename from perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp rename to perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp index 3a5e183767620..26b0204210209 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT -#define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#ifndef VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT +#define VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT +#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include "voxel_grid_map_loader.hpp" #include #include @@ -25,7 +25,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader @@ -70,8 +70,8 @@ class VoxelBasedApproximateCompareMapFilterComponent : public pointcloud_preproc PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelBasedApproximateCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation // clang-format off -#endif // COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#endif // VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT // clang-format on diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp similarity index 92% rename from perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp rename to perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index 449ae76181bc6..1861a710cdebd 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp" +#include "node.hpp" + +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -21,7 +24,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { using pointcloud_preprocessor::get_param; @@ -106,7 +109,8 @@ void VoxelBasedCompareMapFilterComponent::filter( } } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include -RCLCPP_COMPONENTS_REGISTER_NODE(compare_map_segmentation::VoxelBasedCompareMapFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp similarity index 78% rename from perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp rename to perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp index 692193d13b04f..6e03e5d7a2e09 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ -#define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ +#ifndef VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_ +#define VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_ -#include "compare_map_segmentation/voxel_grid_map_loader.hpp" +#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { class VoxelBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter { @@ -45,6 +45,6 @@ class VoxelBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filt PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation -#endif // COMPARE_MAP_SEGMENTATION__VOXEL_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ +#endif // VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_ diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp similarity index 95% rename from perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp rename to perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index 0a660bfffd7fb..7fdeb5d8ea163 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp" +#include "node.hpp" + +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -20,7 +23,7 @@ #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { void VoxelDistanceBasedStaticMapLoader::onMapCallback( @@ -174,8 +177,8 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter( "debug/processing_time_ms", processing_time_ms); } } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include RCLCPP_COMPONENTS_REGISTER_NODE( - compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent) + autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp similarity index 93% rename from perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp rename to perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp index 0cfc8a64ab2dd..e0656359f3df2 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT -#define COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#ifndef VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT +#define VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT +#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include "voxel_grid_map_loader.hpp" #include #include @@ -27,7 +27,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { typedef typename pcl::Filter::PointCloud PointCloud; @@ -139,8 +139,8 @@ class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocess PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelDistanceBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation // clang-format off -#endif // COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#endif // VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT // clang-format on diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp similarity index 99% rename from perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp rename to perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index ef3727019c0f4..b011799e5b912 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/voxel_grid_map_loader.hpp" +#include "voxel_grid_map_loader.hpp" +namespace autoware::compare_map_segmentation +{ VoxelGridMapLoader::VoxelGridMapLoader( rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, std::string * tf_map_input_frame, std::mutex * mutex) @@ -462,3 +464,5 @@ void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Poi } } } + +} // namespace autoware::compare_map_segmentation diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp similarity index 95% rename from perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp rename to perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 21cb19edcd0ec..b13f2e537ee55 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ -#define COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ +#ifndef VOXEL_GRID_MAP_LOADER__VOXEL_GRID_MAP_LOADER_HPP_ +#define VOXEL_GRID_MAP_LOADER__VOXEL_GRID_MAP_LOADER_HPP_ #include -#include +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" #include #include #include @@ -33,6 +33,8 @@ #include #include +namespace autoware::compare_map_segmentation +{ template double distance3D(const T p1, const U p2) { @@ -138,7 +140,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, std::string * tf_map_input_frame, std::mutex * mutex); virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); - virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; }; class VoxelGridDynamicMapLoader : public VoxelGridMapLoader @@ -194,12 +196,12 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group); - void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr pose); + void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg); void timer_callback(); bool should_update_map() const; void request_update_map(const geometry_msgs::msg::Point & position); - virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; /** \brief Check if point close to map pointcloud in the */ bool is_close_to_next_map_grid( const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold); @@ -319,4 +321,6 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader } }; -#endif // COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ +} // namespace autoware::compare_map_segmentation + +#endif // VOXEL_GRID_MAP_LOADER__VOXEL_GRID_MAP_LOADER_HPP_ diff --git a/perception/detected_object_feature_remover/CMakeLists.txt b/perception/detected_object_feature_remover/CMakeLists.txt index b3dd3c35e19d5..e0ff35d75e64a 100644 --- a/perception/detected_object_feature_remover/CMakeLists.txt +++ b/perception/detected_object_feature_remover/CMakeLists.txt @@ -4,13 +4,13 @@ project(detected_object_feature_remover) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(detected_object_feature_remover_node SHARED - src/detected_object_feature_remover.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/detected_object_feature_remover_node.cpp ) -rclcpp_components_register_node(detected_object_feature_remover_node - PLUGIN "detected_object_feature_remover::DetectedObjectFeatureRemover" - EXECUTABLE detected_object_feature_remover +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::detected_object_feature_remover::DetectedObjectFeatureRemover" + EXECUTABLE detected_object_feature_remover_node ) ament_auto_package( diff --git a/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml b/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml index 2e7fa4605d656..d86d75ce297be 100644 --- a/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml +++ b/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.cpp similarity index 86% rename from perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp rename to perception/detected_object_feature_remover/src/detected_object_feature_remover_node.cpp index 3fe9e6fde8683..47fe67de9d9aa 100644 --- a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "detected_object_feature_remover_node.hpp" -namespace detected_object_feature_remover +namespace autoware::detected_object_feature_remover { DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOptions & node_options) : Node("detected_object_feature_remover", node_options) @@ -45,7 +45,8 @@ void DetectedObjectFeatureRemover::convert( } } -} // namespace detected_object_feature_remover +} // namespace autoware::detected_object_feature_remover #include -RCLCPP_COMPONENTS_REGISTER_NODE(detected_object_feature_remover::DetectedObjectFeatureRemover) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_feature_remover::DetectedObjectFeatureRemover) diff --git a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.hpp similarity index 71% rename from perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp rename to perception/detected_object_feature_remover/src/detected_object_feature_remover_node.hpp index 02e4c9c3ca29a..2877d38259f83 100644 --- a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.hpp @@ -12,18 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ -#define DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ +#ifndef DETECTED_OBJECT_FEATURE_REMOVER_NODE_HPP_ +#define DETECTED_OBJECT_FEATURE_REMOVER_NODE_HPP_ + +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" #include -namespace detected_object_feature_remover +namespace autoware::detected_object_feature_remover { using autoware_perception_msgs::msg::DetectedObjects; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; @@ -41,6 +42,6 @@ class DetectedObjectFeatureRemover : public rclcpp::Node void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs); }; -} // namespace detected_object_feature_remover +} // namespace autoware::detected_object_feature_remover -#endif // DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ +#endif // DETECTED_OBJECT_FEATURE_REMOVER_NODE_HPP_ diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index c416209d7d55d..6d93e7b426ad2 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -28,12 +28,8 @@ include_directories( ) # Generate occupancy grid based validator exe file -set(OCCUPANCY_GRID_BASED_VALIDATOR_SRC - src/occupancy_grid_based_validator.cpp -) - ament_auto_add_library(occupancy_grid_based_validator SHARED - ${OCCUPANCY_GRID_BASED_VALIDATOR_SRC} + src/occupancy_grid_map/occupancy_grid_map_validator.cpp ) target_link_libraries(occupancy_grid_based_validator @@ -42,12 +38,8 @@ target_link_libraries(occupancy_grid_based_validator ) # Generate obstacle pointcloud based validator exe file -set(OBSTACLE_POINTCLOUD_BASED_VALIDATOR_SRC - src/obstacle_pointcloud_based_validator.cpp -) - ament_auto_add_library(obstacle_pointcloud_based_validator SHARED - ${OBSTACLE_POINTCLOUD_BASED_VALIDATOR_SRC} + src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp ) target_link_libraries(obstacle_pointcloud_based_validator @@ -56,32 +48,32 @@ target_link_libraries(obstacle_pointcloud_based_validator ) ament_auto_add_library(object_lanelet_filter SHARED - src/object_lanelet_filter.cpp - src/utils.cpp + src/lanelet_filter/lanelet_filter.cpp + lib/utils/utils.cpp ) ament_auto_add_library(object_position_filter SHARED - src/object_position_filter.cpp - src/utils.cpp + src/position_filter/position_filter.cpp + lib/utils/utils.cpp ) rclcpp_components_register_node(obstacle_pointcloud_based_validator - PLUGIN "obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator" + PLUGIN "autoware::detected_object_validation::obstacle_pointcloud::ObstaclePointCloudBasedValidator" EXECUTABLE obstacle_pointcloud_based_validator_node ) rclcpp_components_register_node(object_lanelet_filter - PLUGIN "object_lanelet_filter::ObjectLaneletFilterNode" + PLUGIN "autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode" EXECUTABLE object_lanelet_filter_node ) rclcpp_components_register_node(object_position_filter - PLUGIN "object_position_filter::ObjectPositionFilterNode" + PLUGIN "autoware::detected_object_validation::position_filter::ObjectPositionFilterNode" EXECUTABLE object_position_filter_node ) rclcpp_components_register_node(occupancy_grid_based_validator - PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator" + PLUGIN "autoware::detected_object_validation::occupancy_grid_map::OccupancyGridBasedValidator" EXECUTABLE occupancy_grid_based_validator_node ) diff --git a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp b/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp similarity index 82% rename from perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp rename to perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp index c55e992213eb5..3ec119d2a48c5 100644 --- a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp +++ b/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ -#define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#define AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #include #include + +namespace autoware::detected_object_validation +{ namespace utils { struct FilterTargetLabel @@ -48,5 +51,6 @@ inline bool hasBoundingBox(const autoware_perception_msgs::msg::DetectedObject & } } // namespace utils +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#endif // AUTOWARE__DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/utils.cpp b/perception/detected_object_validation/lib/utils/utils.cpp similarity index 87% rename from perception/detected_object_validation/src/utils.cpp rename to perception/detected_object_validation/lib/utils/utils.cpp index f6e2157cb885f..327f65a0c833e 100644 --- a/perception/detected_object_validation/src/utils.cpp +++ b/perception/detected_object_validation/lib/utils/utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" #include +namespace autoware::detected_object_validation +{ namespace utils { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -27,4 +29,6 @@ bool FilterTargetLabel::isTarget(const uint8_t label) const (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) || (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); } + } // namespace utils +} // namespace autoware::detected_object_validation diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index d8eb1257ba451..09440c9f6358d 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -22,17 +22,14 @@ autoware_perception_msgs autoware_test_utils autoware_universe_utils - geometry_msgs message_filters nav_msgs object_recognition_utils pcl_conversions rclcpp rclcpp_components - tf2 tf2_geometry_msgs tf2_ros - tier4_perception_msgs ament_lint_auto autoware_lint_common diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp similarity index 95% rename from perception/detected_object_validation/src/object_lanelet_filter.cpp rename to perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index d1ba34f4a61b2..09440fedc1764 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" +#include "lanelet_filter.hpp" -#include -#include -#include -#include +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_lanelet2_extension/utility/message_conversion.hpp" +#include "autoware_lanelet2_extension/utility/query.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -25,7 +25,9 @@ #include -namespace object_lanelet_filter +namespace autoware::detected_object_validation +{ +namespace lanelet_filter { ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options) : Node("object_lanelet_filter_node", node_options), @@ -309,7 +311,9 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( return false; } -} // namespace object_lanelet_filter +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_lanelet_filter::ObjectLaneletFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp similarity index 81% rename from perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp rename to perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 3ffd8d18c9a67..25d78a160c246 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#ifndef LANELET_FILTER__LANELET_FILTER_HPP_ +#define LANELET_FILTER__LANELET_FILTER_HPP_ -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware_lanelet2_extension/utility/utilities.hpp" -#include -#include -#include -#include #include -#include -#include +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -33,7 +33,9 @@ #include #include -namespace object_lanelet_filter +namespace autoware::detected_object_validation +{ +namespace lanelet_filter { using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::MultiPoint2d; @@ -92,6 +94,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_lanelet_filter +} // namespace lanelet_filter +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#endif // LANELET_FILTER__LANELET_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp similarity index 89% rename from perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp rename to perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp index d338580d95418..5886474d51b34 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp @@ -12,19 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ -#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#ifndef OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ +#define OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include #include #include -namespace obstacle_pointcloud_based_validator +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { class Debugger { @@ -106,6 +108,8 @@ class Debugger return pointcloud_xyz; } }; -} // namespace obstacle_pointcloud_based_validator -#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation + +#endif // OBSTACLE_POINTCLOUD__DEBUGGER_HPP_ diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp similarity index 97% rename from perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp rename to perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index fbb3f863be7c4..c5ecce714735b 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#define EIGEN_MPL2_ONLY + +#include "obstacle_pointcloud_validator.hpp" #include #include @@ -25,17 +27,18 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include -namespace obstacle_pointcloud_based_validator +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { namespace bg = boost::geometry; using Shape = autoware_perception_msgs::msg::Shape; using Polygon2d = autoware::universe_utils::Polygon2d; -Validator::Validator(PointsNumThresholdParam & points_num_threshold_param) +Validator::Validator(const PointsNumThresholdParam & points_num_threshold_param) { points_num_threshold_param_.min_points_num = points_num_threshold_param.min_points_num; points_num_threshold_param_.max_points_num = points_num_threshold_param.max_points_num; @@ -369,8 +372,9 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( "debug/pipeline_latency_ms", pipeline_latency); } -} // namespace obstacle_pointcloud_based_validator +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation #include RCLCPP_COMPONENTS_REGISTER_NODE( - obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator) + autoware::detected_object_validation::obstacle_pointcloud::ObstaclePointCloudBasedValidator) diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp similarity index 76% rename from perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp rename to perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index a16bb63fd0c87..801a693905f93 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -13,17 +13,17 @@ // limitations under the License. // NOLINTNEXTLINE(whitespace/line_length) -#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#ifndef OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ // NOLINTNEXTLINE(whitespace/line_length) -#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#define OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ -#include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "debugger.hpp" -#include -#include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -42,7 +42,10 @@ #include #include #include -namespace obstacle_pointcloud_based_validator + +namespace autoware::detected_object_validation +{ +namespace obstacle_pointcloud { struct PointsNumThresholdParam @@ -61,7 +64,7 @@ class Validator pcl::PointCloud::Ptr cropped_pointcloud_; public: - explicit Validator(PointsNumThresholdParam & points_num_threshold_param); + explicit Validator(const PointsNumThresholdParam & points_num_threshold_param); inline pcl::PointCloud::Ptr getDebugPointCloudWithinObject() { return cropped_pointcloud_; @@ -91,17 +94,20 @@ class Validator2D : public Validator pcl::PointCloud::Ptr convertToXYZ( const pcl::PointCloud::Ptr & pointcloud_xy); - inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { return convertToXYZ(neighbor_pointcloud_); } - bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); - bool validate_object(const autoware_perception_msgs::msg::DetectedObject & transformed_object); - std::optional getMaxRadius(const autoware_perception_msgs::msg::DetectedObject & object); + bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) override; + bool validate_object( + const autoware_perception_msgs::msg::DetectedObject & transformed_object) override; + std::optional getMaxRadius( + const autoware_perception_msgs::msg::DetectedObject & object) override; std::optional getPointCloudWithinObject( const autoware_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr neighbor_pointcloud); + const pcl::PointCloud::Ptr pointcloud); }; class Validator3D : public Validator { @@ -112,13 +118,16 @@ class Validator3D : public Validator public: explicit Validator3D(PointsNumThresholdParam & points_num_threshold_param); - inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { return neighbor_pointcloud_; } - bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); - bool validate_object(const autoware_perception_msgs::msg::DetectedObject & transformed_object); - std::optional getMaxRadius(const autoware_perception_msgs::msg::DetectedObject & object); + bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) override; + bool validate_object( + const autoware_perception_msgs::msg::DetectedObject & transformed_object) override; + std::optional getMaxRadius( + const autoware_perception_msgs::msg::DetectedObject & object) override; std::optional getPointCloudWithinObject( const autoware_perception_msgs::msg::DetectedObject & object, const pcl::PointCloud::Ptr neighbor_pointcloud); @@ -154,7 +163,9 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); }; -} // namespace obstacle_pointcloud_based_validator + +} // namespace obstacle_pointcloud +} // namespace autoware::detected_object_validation // NOLINTNEXTLINE(whitespace/line_length) -#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +#endif // OBSTACLE_POINTCLOUD__OBSTACLE_POINTCLOUD_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp similarity index 92% rename from perception/detected_object_validation/src/occupancy_grid_based_validator.cpp rename to perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index b8682a6e56b3b..ce900f30f4255 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#define EIGEN_MPL2_ONLY + +#include "occupancy_grid_map_validator.hpp" -#include -#include -#include +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "object_recognition_utils/object_classification.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -26,11 +28,12 @@ #include #endif -#define EIGEN_MPL2_ONLY #include #include -namespace occupancy_grid_based_validator +namespace autoware::detected_object_validation +{ +namespace occupancy_grid_map { using Shape = autoware_perception_msgs::msg::Shape; using Polygon2d = autoware::universe_utils::Polygon2d; @@ -173,7 +176,9 @@ void OccupancyGridBasedValidator::showDebugImage( cv::waitKey(2); } -} // namespace occupancy_grid_based_validator +} // namespace occupancy_grid_map +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_based_validator::OccupancyGridBasedValidator) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::occupancy_grid_map::OccupancyGridBasedValidator) diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp similarity index 83% rename from perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp rename to perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp index beb8faf5db1a3..52b6f79e5e20a 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ -#define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#ifndef OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ +#define OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ + +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include #include #include #include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -30,7 +31,9 @@ #include #include -namespace occupancy_grid_based_validator +namespace autoware::detected_object_validation +{ +namespace occupancy_grid_map { class OccupancyGridBasedValidator : public rclcpp::Node { @@ -68,6 +71,8 @@ class OccupancyGridBasedValidator : public rclcpp::Node const nav_msgs::msg::OccupancyGrid & ros_occ_grid, const autoware_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid); }; -} // namespace occupancy_grid_based_validator -#endif // DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +} // namespace occupancy_grid_map +} // namespace autoware::detected_object_validation + +#endif // OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/position_filter/position_filter.cpp similarity index 91% rename from perception/detected_object_validation/src/object_position_filter.cpp rename to perception/detected_object_validation/src/position_filter/position_filter.cpp index dccff8a6ccc67..ac0bab404c476 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/position_filter/position_filter.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" +#include "position_filter.hpp" -namespace object_position_filter +namespace autoware::detected_object_validation +{ +namespace position_filter { ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options) : Node("object_position_filter_node", node_options), @@ -78,7 +80,9 @@ bool ObjectPositionFilterNode::isObjectInBounds( position.y > lower_bound_y_ && position.y < upper_bound_y_; } -} // namespace object_position_filter +} // namespace position_filter +} // namespace autoware::detected_object_validation #include -RCLCPP_COMPONENTS_REGISTER_NODE(object_position_filter::ObjectPositionFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::detected_object_validation::position_filter::ObjectPositionFilterNode) diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/src/position_filter/position_filter.hpp similarity index 71% rename from perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp rename to perception/detected_object_validation/src/position_filter/position_filter.hpp index ea70d62fd952d..30a5d7fc9dfb1 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/src/position_filter/position_filter.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#ifndef POSITION_FILTER__POSITION_FILTER_HPP_ +#define POSITION_FILTER__POSITION_FILTER_HPP_ -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include -#include #include -#include -#include +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include @@ -30,7 +30,9 @@ #include #include -namespace object_position_filter +namespace autoware::detected_object_validation +{ +namespace position_filter { class ObjectPositionFilterNode : public rclcpp::Node @@ -57,6 +59,7 @@ class ObjectPositionFilterNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace object_position_filter +} // namespace position_filter +} // namespace autoware::detected_object_validation -#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#endif // POSITION_FILTER__POSITION_FILTER_HPP_ diff --git a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp b/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp index 03cebf91418ce..5bd174d5a9e17 100644 --- a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp +++ b/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" +#include "../../src/position_filter/position_filter.hpp" #include #include @@ -21,10 +21,10 @@ #include +using autoware::detected_object_validation::position_filter::ObjectPositionFilterNode; using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using autoware_perception_msgs::msg::ObjectClassification; -using object_position_filter::ObjectPositionFilterNode; std::shared_ptr generateTestManager() { diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp index f7077dadb1ef2..7865f928f23a8 100644 --- a/perception/detected_object_validation/test/test_utils.cpp +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_validation/utils/utils.hpp" +#include "autoware/detected_object_validation/utils/utils.hpp" -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" #include +using autoware::detected_object_validation::utils::FilterTargetLabel; using AutowareLabel = autoware_perception_msgs::msg::ObjectClassification; -utils::FilterTargetLabel createFilterTargetAll() +FilterTargetLabel createFilterTargetAll() { - utils::FilterTargetLabel filter; + FilterTargetLabel filter; filter.UNKNOWN = true; filter.CAR = true; filter.TRUCK = true; @@ -34,9 +35,9 @@ utils::FilterTargetLabel createFilterTargetAll() return filter; } -utils::FilterTargetLabel createFilterTargetVehicle() +FilterTargetLabel createFilterTargetVehicle() { - utils::FilterTargetLabel filter; + FilterTargetLabel filter; filter.UNKNOWN = false; filter.CAR = true; filter.TRUCK = true; diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index fe1fa9fda5fca..d27390ae7707a 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -13,49 +13,57 @@ include_directories( ${PCL_INCLUDE_DIRS} ) -ament_auto_add_library(cluster_lib SHARED - lib/utils.cpp +ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/euclidean_cluster.cpp lib/voxel_grid_based_euclidean_cluster.cpp + lib/utils.cpp ) -target_link_libraries(cluster_lib +target_link_libraries(${PROJECT_NAME}_lib ${PCL_LIBRARIES} ) -target_include_directories(cluster_lib +target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ ) -ament_auto_add_library(euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_node_core SHARED src/euclidean_cluster_node.cpp ) -target_link_libraries(euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(euclidean_cluster_node_core - PLUGIN "euclidean_cluster::EuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_node_core + PLUGIN "autoware::euclidean_cluster::EuclideanClusterNode" EXECUTABLE euclidean_cluster_node ) -ament_auto_add_library(voxel_grid_based_euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_voxel_grid_node_core SHARED src/voxel_grid_based_euclidean_cluster_node.cpp ) -target_link_libraries(voxel_grid_based_euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_voxel_grid_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(voxel_grid_based_euclidean_cluster_node_core - PLUGIN "euclidean_cluster::VoxelGridBasedEuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_voxel_grid_node_core + PLUGIN "autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode" EXECUTABLE voxel_grid_based_euclidean_cluster_node ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_voxel_grid_based_euclidean_cluster_fusion + test/test_voxel_grid_based_euclidean_cluster.cpp + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config + test ) diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp similarity index 87% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp index aec3f56936828..8c4aad537cc43 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp @@ -14,14 +14,14 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanCluster : public EuclideanClusterInterface { @@ -42,4 +42,4 @@ class EuclideanCluster : public EuclideanClusterInterface float tolerance_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp similarity index 95% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp index 65b907f747666..461d75d4931db 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanClusterInterface { @@ -54,4 +54,4 @@ class EuclideanClusterInterface int max_cluster_size_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp similarity index 94% rename from perception/euclidean_cluster/include/euclidean_cluster/utils.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp index 50d2306d48f72..a52c0840c4d41 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); void convertPointCloudClusters2Msg( @@ -37,4 +37,4 @@ void convertPointCloudClusters2Msg( void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output); -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp similarity index 90% rename from perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp index 375cc2d19a01f..5c51bb85ce177 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -14,15 +14,15 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface { @@ -52,4 +52,4 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface int min_points_number_per_voxel_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index b8d4f61a9cf28..053438a4352b0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", "low_height/pointcloud"), @@ -60,7 +60,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 607e1bf30ccaa..0b502b1c43d67 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", "low_height/pointcloud"), @@ -61,7 +61,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index 5ba1b99403280..0481b7e1b742d 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanCluster::EuclideanCluster() { @@ -93,4 +93,4 @@ bool EuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 6d119ef3d01aa..624c838ef0647 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include @@ -19,7 +19,7 @@ #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) { @@ -128,4 +128,4 @@ void convertObjectMsg2SensorMsg( output.height = 1; output.is_dense = false; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index 126f877afddb0..096de3dc1b296 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster() { @@ -166,4 +166,4 @@ bool VoxelGridBasedEuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index c5cdd9c54b988..45b5d05031c72 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -12,7 +12,8 @@ autoware_cmake autoware_perception_msgs - compare_map_segmentation + autoware_point_types + autoware_universe_utils geometry_msgs libpcl-all-dev pcl_conversions @@ -21,6 +22,8 @@ sensor_msgs tier4_perception_msgs + ament_cmake_gtest + ament_lint_auto autoware_lint_common diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index f986b0334935f..b0b9448dfc0c4 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) : Node("euclidean_cluster_node", options) @@ -86,8 +86,8 @@ void EuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::EuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::EuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index 92f10696d17dc..5ab48abb4f042 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include @@ -26,7 +26,8 @@ #include #include -namespace euclidean_cluster + +namespace autoware::euclidean_cluster { class EuclideanClusterNode : public rclcpp::Node { @@ -45,4 +46,4 @@ class EuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index e9425095a3b0d..e54c55e4873ee 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "voxel_grid_based_euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( const rclcpp::NodeOptions & options) @@ -89,8 +89,8 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::VoxelGridBasedEuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index b0c5d0e5a7fbf..b48e30b37de04 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include @@ -26,7 +26,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node { @@ -45,4 +45,4 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp new file mode 100644 index 0000000000000..7d2edbe0de0d7 --- /dev/null +++ b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp @@ -0,0 +1,173 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" + +#include +#include + +#include +#include +#include + +#include + +using autoware_point_types::PointXYZI; +void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud) +{ + pointcloud.fields.resize(4); + pointcloud.fields[0].name = "x"; + pointcloud.fields[1].name = "y"; + pointcloud.fields[2].name = "z"; + pointcloud.fields[3].name = "intensity"; + pointcloud.fields[0].offset = 0; + pointcloud.fields[1].offset = 4; + pointcloud.fields[2].offset = 8; + pointcloud.fields[3].offset = 12; + pointcloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[0].count = 1; + pointcloud.fields[1].count = 1; + pointcloud.fields[2].count = 1; + pointcloud.fields[3].count = 1; + pointcloud.height = 1; + pointcloud.point_step = 16; + pointcloud.is_bigendian = false; + pointcloud.is_dense = true; + pointcloud.header.frame_id = "dummy_frame_id"; + pointcloud.header.stamp.sec = 0; + pointcloud.header.stamp.nanosec = 0; +} + +sensor_msgs::msg::PointCloud2 generateClusterWithinVoxel(const int nb_points) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + pointcloud.data.resize(nb_points * pointcloud.point_step); + + // generate one cluster with specified number of points within 1 voxel + for (int i = 0; i < nb_points; ++i) { + PointXYZI point; + point.x = std::experimental::randint(0, 30) / 100.0; // point.x within 0.0 to 0.3 + point.y = std::experimental::randint(0, 30) / 100.0; // point.y within 0.0 to 0.3 + point.z = std::experimental::randint(0, 30) / 1.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[i * pointcloud.point_step], &point, pointcloud.point_step); + } + pointcloud.width = nb_points; + pointcloud.row_step = pointcloud.point_step * nb_points; + return pointcloud; +} + +// Test case 1: Test case when the input pointcloud has only one cluster with points number equal to +// max_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase1) +{ + int nb_generated_points = 100; + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 1; + int max_cluster_size = 100; + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + std::cout << "number points of first cluster: " << output.feature_objects[0].feature.cluster.width + << std::endl; + // the output clusters should has only one cluster with nb_generated_points points + EXPECT_EQ(output.feature_objects.size(), 1); + EXPECT_EQ(output.feature_objects[0].feature.cluster.width, nb_generated_points); +} + +// Test case 2: Test case when the input pointcloud has only one cluster with points number less +// than min_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase2) +{ + int nb_generated_points = 1; + + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 2; + int max_cluster_size = 100; + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + // the output clusters should be empty + EXPECT_EQ(output.feature_objects.size(), 0); +} + +// Test case 3: Test case when the input pointcloud has two clusters with points number greater to +// max_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase3) +{ + int nb_generated_points = 100; + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 1; + int max_cluster_size = 99; // max_cluster_size is less than nb_generated_points + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + // the output clusters should be emtpy + EXPECT_EQ(output.feature_objects.size(), 0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/ground_segmentation/src/ray_ground_filter/node.cpp b/perception/ground_segmentation/src/ray_ground_filter/node.cpp index c75972d3c4f14..f07db15615739 100644 --- a/perception/ground_segmentation/src/ray_ground_filter/node.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter/node.cpp @@ -259,7 +259,8 @@ void RayGroundFilterComponent::ClassifyPointCloud( // } void RayGroundFilterComponent::initializePointCloud2( - const PointCloud2::ConstSharedPtr & in_cloud_ptr, PointCloud2::SharedPtr & out_cloud_msg_ptr) + const PointCloud2::ConstSharedPtr & in_cloud_ptr, + const PointCloud2::SharedPtr & out_cloud_msg_ptr) { out_cloud_msg_ptr->header = in_cloud_ptr->header; out_cloud_msg_ptr->height = in_cloud_ptr->height; @@ -271,7 +272,7 @@ void RayGroundFilterComponent::initializePointCloud2( } void RayGroundFilterComponent::ExtractPointsIndices( - const PointCloud2::ConstSharedPtr in_cloud_ptr, pcl::PointIndices & in_indices, + const PointCloud2::ConstSharedPtr in_cloud_ptr, const pcl::PointIndices & in_indices, PointCloud2::SharedPtr ground_cloud_msg_ptr, PointCloud2::SharedPtr no_ground_cloud_msg_ptr) { initializePointCloud2(in_cloud_ptr, ground_cloud_msg_ptr); diff --git a/perception/ground_segmentation/src/ray_ground_filter/node.hpp b/perception/ground_segmentation/src/ray_ground_filter/node.hpp index 8bb17cdaf8a09..d3be2e60c4262 100644 --- a/perception/ground_segmentation/src/ray_ground_filter/node.hpp +++ b/perception/ground_segmentation/src/ray_ground_filter/node.hpp @@ -185,7 +185,7 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter * @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed */ void ExtractPointsIndices( - const PointCloud2::ConstSharedPtr in_cloud_ptr, pcl::PointIndices & in_indices, + const PointCloud2::ConstSharedPtr in_cloud_ptr, const pcl::PointIndices & in_indices, PointCloud2::SharedPtr out_only_indices_cloud_ptr, PointCloud2::SharedPtr out_removed_indices_cloud_ptr); @@ -194,7 +194,8 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter void setVehicleFootprint( const double min_x, const double max_x, const double min_y, const double max_y); void initializePointCloud2( - const PointCloud2::ConstSharedPtr & in_cloud_ptr, PointCloud2::SharedPtr & out_cloud_msg_ptr); + const PointCloud2::ConstSharedPtr & in_cloud_ptr, + const PointCloud2::SharedPtr & out_cloud_msg_ptr); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/perception/ground_segmentation/src/scan_ground_filter/node.cpp b/perception/ground_segmentation/src/scan_ground_filter/node.cpp index 26db036f79bbe..01116c5bdce5f 100644 --- a/perception/ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter/node.cpp @@ -96,8 +96,9 @@ inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstP int intensity_index = pcl::getFieldIndex(*input, "intensity"); if (intensity_index != -1) { intensity_offset_ = input->fields[intensity_index].offset; + intensity_type_ = input->fields[intensity_index].datatype; } else { - intensity_offset_ = z_offset_ + sizeof(float); + intensity_offset_ = -1; } offset_initialized_ = true; } @@ -244,7 +245,7 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { float next_gnd_z = 0.0f; float curr_gnd_slope_ratio = 0.0f; @@ -286,7 +287,7 @@ void ScanGroundFilterComponent::checkContinuousGndGrid( } void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { float tmp_delta_max_z = p_orig_point.z - gnd_grids_list.back().max_height; float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; @@ -304,7 +305,7 @@ void ScanGroundFilterComponent::checkDiscontinuousGndGrid( } void ScanGroundFilterComponent::checkBreakGndGrid( - PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) + PointData & p, const pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; @@ -569,6 +570,7 @@ void ScanGroundFilterComponent::extractObjectPoints( PointCloud2 & out_object_cloud) { size_t output_data_size = 0; + for (const auto & i : in_indices.indices) { std::memcpy( &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], diff --git a/perception/ground_segmentation/src/scan_ground_filter/node.hpp b/perception/ground_segmentation/src/scan_ground_filter/node.hpp index 0e6ed598053ba..67a844eee75c5 100644 --- a/perception/ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/ground_segmentation/src/scan_ground_filter/node.hpp @@ -161,6 +161,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter int y_offset_; int z_offset_; int intensity_offset_; + int intensity_type_; bool offset_initialized_; void set_field_offsets(const PointCloud2ConstPtr & input); @@ -239,11 +240,14 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter const float h, const float r, const uint16_t id, std::vector & gnd_grids); void checkContinuousGndGrid( - PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + PointData & p, const pcl::PointXYZ & p_orig_point, + const std::vector & gnd_grids_list); void checkDiscontinuousGndGrid( - PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + PointData & p, const pcl::PointXYZ & p_orig_point, + const std::vector & gnd_grids_list); void checkBreakGndGrid( - PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + PointData & p, const pcl::PointXYZ & p_orig_point, + const std::vector & gnd_grids_list); void classifyPointCloud( const PointCloud2ConstPtr & in_cloud_ptr, std::vector & in_radial_ordered_clouds, diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index b6dfbaf402cc8..6be7defe422c3 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -172,6 +172,13 @@ if(BUILD_TESTING) ament_auto_add_gtest(test_geometry test/test_geometry.cpp ) + # test needed cuda, tensorRT and cudnn + if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + ament_auto_add_gtest(test_pointpainting + test/test_pointpainting_fusion.cpp + ) + endif() + endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 30136bc8f47d0..9227e4fa9319a 100644 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -5,6 +5,7 @@ head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" trt_precision: fp16 + cloud_capacity: 2000000 post_process_params: # post-process params circle_nms_dist_threshold: 0.3 diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 2120a909cd672..fdabb0c7055d8 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -1,39 +1,24 @@ /**: ros__parameters: # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: - [ - true, # road - true, # sidewalk - true, # building - true, # wall - true, # fence - true, # pole - true, # traffic_light - true, # traffic_sign - true, # vegetation - true, # terrain - true, # sky - false, # person - false, # ride - false, # car - false, # truck - false, # bus - false, # train - false, # motorcycle - false, # bicycle - false, # others - ] - # the maximum distance of pointcloud to be applied filter, - # this is selected based on semantic segmentation model accuracy, - # calibration accuracy and unknown reaction distance - filter_distance_threshold: 60.0 + UNKNOWN: false + BUILDING: true + WALL: true + OBSTACLE: false + TRAFFIC_LIGHT: false + TRAFFIC_SIGN: false + PERSON: false + VEHICLE: false + BIKE: false + ROAD: true + SIDEWALK: false + ROAD_PAINT: false + CURBSTONE: false + CROSSWALK: false + VEGETATION: true + SKY: false - # debug - debug_mode: false - filter_scope_min_x: -100.0 - filter_scope_max_x: 100.0 - filter_scope_min_y: -100.0 - filter_scope_max_y: 100.0 - filter_scope_min_z: -100.0 - filter_scope_max_z: 100.0 + # the maximum distance of pointcloud to be applied filter + filter_distance_threshold: 60.0 diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md index d59e804f1228c..3c469ac15c6e7 100644 --- a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -32,9 +32,7 @@ The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud ### Core Parameters -| Name | Type | Description | -| ------------- | ---- | ------------------------ | -| `rois_number` | int | the number of input rois | +{{ json_to_markdown("perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }} ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index a4e6f315a36d6..756cb4224bc20 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -33,6 +33,14 @@ namespace image_projection_based_fusion { using Label = autoware_perception_msgs::msg::ObjectClassification; +inline bool isInsideBbox( + float proj_x, float proj_y, sensor_msgs::msg::RegionOfInterest roi, float zc) +{ + // z_c is scaling to normalize projection point + return proj_x >= roi.x_offset * zc && proj_x <= (roi.x_offset + roi.width) * zc && + proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; +} + class PointPaintingFusionNode : public FusionNode { diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index d28d9eb31216d..c8318f79bbeed 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -23,7 +23,6 @@ namespace image_projection_based_fusion { -static constexpr size_t CAPACITY_POINT = 1000000; class PointPaintingTRT : public centerpoint::CenterPointTRT { public: diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 26bcfd5ed802a..89b33775f7898 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #if __has_include() @@ -36,7 +37,13 @@ class SegmentPointCloudFusionNode : public FusionNode::SharedPtr pub_pointcloud_ptr_; std::vector filter_semantic_label_target_; float filter_distance_threshold_; - /* data */ + // declare list of semantic label target, depend on trained data of yolox segmentation model + std::vector> filter_semantic_label_target_list_ = { + {"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false}, + {"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false}, + {"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROAD_PAINT", false}, + {"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}}; + public: explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json new file mode 100644 index 0000000000000..f5ab1be5eac34 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json @@ -0,0 +1,134 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Segmentation Pointcloud Fusion Node", + "type": "object", + "definitions": { + "segmentation_pointcloud_fusion": { + "type": "object", + "properties": { + "filter_semantic_label_target": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "boolean", + "description": "If true, UNKNOWN class of semantic will be filtered.", + "default": false + }, + "BUILDING": { + "type": "boolean", + "description": "If true, BUILDING class of semantic will be filtered.", + "default": true + }, + "WALL": { + "type": "boolean", + "description": "If true, WALL class of semantic will be filtered.", + "default": true + }, + "OBSTACLE": { + "type": "boolean", + "description": "If true, OBSTACLE class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_LIGHT": { + "type": "boolean", + "description": "If true, TRAFFIC_LIGHT class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_SIGN": { + "type": "boolean", + "description": "If true, TRAFFIC_SIGN class of semantic will be filtered.", + "default": false + }, + "PERSON": { + "type": "boolean", + "description": "If true, PERSON class of semantic will be filtered.", + "default": false + }, + "VEHICLE": { + "type": "boolean", + "description": "If true, VEHICLE class of semantic will be filtered.", + "default": false + }, + "BIKE": { + "type": "boolean", + "description": "If true, BIKE class of semantic will be filtered.", + "default": false + }, + "ROAD": { + "type": "boolean", + "description": "If true, ROAD class of semantic will be filtered.", + "default": true + }, + "SIDEWALK": { + "type": "boolean", + "description": "If true, SIDEWALK class of semantic will be filtered.", + "default": false + }, + "ROAD_PAINT": { + "type": "boolean", + "description": "If true, ROAD_PAINT class of semantic will be filtered.", + "default": false + }, + "CURBSTONE": { + "type": "boolean", + "description": "If true, CURBSTONE class of semantic will be filtered.", + "default": false + }, + "CROSSWALK": { + "type": "boolean", + "description": "If true, CROSSWALK class of semantic will be filtered.", + "default": false + }, + "VEGETATION": { + "type": "boolean", + "description": "If true, VEGETATION class of semantic will be filtered.", + "default": true + }, + "SKY": { + "type": "boolean", + "description": "If true, SKY class of semantic will be filtered.", + "default": false + } + }, + "required": [ + "UNKNOWN", + "BUILDING", + "WALL", + "OBSTACLE", + "TRAFFIC_LIGHT", + "TRAFFIC_SIGN", + "PERSON", + "VEHICLE", + "BIKE", + "ROAD", + "SIDEWALK", + "ROAD_PAINT", + "CURBSTONE", + "CROSSWALK", + "VEGETATION", + "SKY" + ] + }, + "filter_distance_threshold": { + "type": "number", + "description": "A maximum distance of pointcloud to apply filter [m].", + "default": 60.0, + "minimum": 0.0 + } + }, + "required": ["filter_semantic_label_target", "filter_distance_threshold"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/segmentation_pointcloud_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index a7c83ac7614a8..6947331d6bcfa 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -45,13 +45,6 @@ Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) namespace image_projection_based_fusion { -inline bool isInsideBbox( - float proj_x, float proj_y, sensor_msgs::msg::RegionOfInterest roi, float zc) -{ - return proj_x >= roi.x_offset * zc && proj_x <= (roi.x_offset + roi.width) * zc && - proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; -} - inline bool isVehicle(int label2d) { return label2d == autoware_perception_msgs::msg::ObjectClassification::CAR || @@ -110,6 +103,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt this->declare_parameter("densification_params.num_past_frames"); // network param const std::string trt_precision = this->declare_parameter("trt_precision"); + const std::size_t cloud_capacity = this->declare_parameter("cloud_capacity"); const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); const std::string encoder_engine_path = this->declare_parameter("encoder_engine_path"); @@ -188,9 +182,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt centerpoint::DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); centerpoint::CenterPointConfig config( - class_names_.size(), point_feature_size, max_voxel_size, pointcloud_range, voxel_size, - downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, - yaw_norm_thresholds, has_variance_); + class_names_.size(), point_feature_size, cloud_capacity, max_voxel_size, pointcloud_range, + voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, + circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_); // create detector detector_ptr_ = std::make_unique( @@ -296,13 +290,13 @@ void PointPaintingFusionNode::fuseOnSingleImage( // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); const auto x_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::X)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::X)) .offset; const auto y_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Y)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::Y)) .offset; const auto z_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Z)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::Z)) .offset; const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; const auto p_step = painted_pointcloud_msg.point_step; @@ -329,9 +323,11 @@ dc | dc dc dc dc ||zc| int stride = p_step * i; unsigned char * data = &painted_pointcloud_msg.data[0]; unsigned char * output = &painted_pointcloud_msg.data[0]; + // cppcheck-suppress-begin invalidPointerCast float p_x = *reinterpret_cast(&data[stride + x_offset]); float p_y = *reinterpret_cast(&data[stride + y_offset]); float p_z = *reinterpret_cast(&data[stride + z_offset]); + // cppcheck-suppress-end invalidPointerCast point_lidar << p_x, p_y, p_z; point_camera = lidar2cam_affine * point_lidar; p_x = point_camera.x(); @@ -352,6 +348,7 @@ dc | dc dc dc dc ||zc| int label2d = feature_object.object.classification.front().label; if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, p_z)) { data = &painted_pointcloud_msg.data[0]; + // cppcheck-suppress invalidPointerCast auto p_class = reinterpret_cast(&output[stride + class_offset]); for (const auto & cls : isClassTable_) { // add up the class values if the point belongs to multiple classes diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 7f19402d9e565..904e66cb53298 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -25,7 +25,7 @@ #include #endif -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" namespace image_projection_based_fusion { @@ -68,7 +68,7 @@ void RoiPointCloudFusionNode::postprocess( // publish debug cluster if (cluster_debug_pub_->get_subscription_count() > 0) { sensor_msgs::msg::PointCloud2 debug_cluster_msg; - euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); cluster_debug_pub_->publish(debug_cluster_msg); } } diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index a3117125a46b1..9a983252af436 100644 --- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -31,8 +31,13 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio : FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); - filter_semantic_label_target_ = - declare_parameter>("filter_semantic_label_target"); + for (auto & item : filter_semantic_label_target_list_) { + item.second = declare_parameter("filter_semantic_label_target." + item.first); + } + for (const auto & item : filter_semantic_label_target_list_) { + RCLCPP_INFO( + this->get_logger(), "filter_semantic_label_target: %s %d", item.first.c_str(), item.second); + } } void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) @@ -129,12 +134,12 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( // skip filtering pointcloud where semantic id out of the defined list uint8_t semantic_id = mask.at( static_cast(projected_point.y()), static_cast(projected_point.x())); - if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + if (static_cast(semantic_id) >= filter_semantic_label_target_list_.size()) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } - if (!filter_semantic_label_target_.at(semantic_id)) { + if (!filter_semantic_label_target_list_.at(semantic_id).second) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); } diff --git a/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp b/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp new file mode 100644 index 0000000000000..1c69273667997 --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp @@ -0,0 +1,56 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +sensor_msgs::msg::RegionOfInterest createRoi( + const int32_t x_offset, const int32_t y_offset, const int32_t width, const int32_t height) +{ + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = x_offset; + roi.y_offset = y_offset; + roi.width = width; + roi.height = height; + return roi; +} + +TEST(isInsideBboxTest, Inside) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(25.0, 25.0, roi, 1.0); + EXPECT_TRUE(result); +} + +TEST(isInsideBboxTest, Border) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(20.0, 30.0, roi, 1.0); + EXPECT_TRUE(result); +} + +TEST(isInsideBboxTest, Outside) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(20, 20, 10, 10); + bool result = image_projection_based_fusion::isInsideBbox(15.0, 15.0, roi, 1.0); + EXPECT_FALSE(result); +} + +TEST(isInsideBboxTest, Zero) +{ + const sensor_msgs::msg::RegionOfInterest roi = createRoi(0, 0, 0, 0); + bool result = image_projection_based_fusion::isInsideBbox(0.0, 0.0, roi, 1.0); + EXPECT_TRUE(result); +} diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index b704bab39338a..ba4d7f788b0f5 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -16,6 +16,8 @@ #include "lidar_apollo_instance_segmentation/feature_map.hpp" +#include + #include #include #include @@ -126,7 +128,27 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( // convert from ros to pcl pcl::PointCloud::Ptr pcl_pointcloud_raw_ptr(new pcl::PointCloud); - pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + // pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + + auto pcl_pointcloud_raw = *pcl_pointcloud_raw_ptr; + pcl_pointcloud_raw.width = transformed_cloud.width; + pcl_pointcloud_raw.height = transformed_cloud.height; + pcl_pointcloud_raw.is_dense = transformed_cloud.is_dense == 1; + pcl_pointcloud_raw.resize(transformed_cloud.width * transformed_cloud.height); + + sensor_msgs::PointCloud2ConstIterator it_x(transformed_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator it_y(transformed_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator it_z(transformed_cloud, "z"); + sensor_msgs::PointCloud2ConstIterator it_intensity(transformed_cloud, "intensity"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { + pcl::PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = static_cast(*it_intensity); + pcl_pointcloud_raw.emplace_back(std::move(point)); + } // generate feature map std::shared_ptr feature_map_ptr = diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 9d41770eb5331..32211bf84efec 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include #include @@ -168,7 +170,29 @@ std::shared_ptr ApolloLidarSegmentation::detec sensor_msgs::msg::PointCloud2 transformed_cloud; ApolloLidarSegmentation::transformCloud(input, transformed_cloud, z_offset_); // convert from ros to pcl - pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_ptr_); + // pcl::fromROSMsg( + // transformed_cloud, *pcl_pointcloud_ptr_); // Manual conversion is needed since intensity + // comes as an uint8_t + + auto pcl_pointcloud = *pcl_pointcloud_ptr_; + pcl_pointcloud.width = input.width; + pcl_pointcloud.height = input.height; + pcl_pointcloud.is_dense = input.is_dense == 1; + pcl_pointcloud.resize(input.width * input.height); + + sensor_msgs::PointCloud2ConstIterator it_x(input, "x"); + sensor_msgs::PointCloud2ConstIterator it_y(input, "y"); + sensor_msgs::PointCloud2ConstIterator it_z(input, "z"); + sensor_msgs::PointCloud2ConstIterator it_intensity(input, "intensity"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { + pcl::PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = static_cast(*it_intensity); + pcl_pointcloud.emplace_back(std::move(point)); + } // inference pipeline auto output = pipeline->schedule(pcl_pointcloud_ptr_); @@ -178,10 +202,10 @@ std::shared_ptr ApolloLidarSegmentation::detec output->header = input.header; // move down pointcloud z_offset in z axis - for (std::size_t i = 0; i < output->feature_objects.size(); i++) { - sensor_msgs::msg::PointCloud2 transformed_cloud; - transformCloud(output->feature_objects.at(i).feature.cluster, transformed_cloud, -z_offset_); - output->feature_objects.at(i).feature.cluster = transformed_cloud; + for (auto & feature_object : output->feature_objects) { + sensor_msgs::msg::PointCloud2 updated_cloud; + transformCloud(feature_object.feature.cluster, updated_cloud, -z_offset_); + feature_object.feature.cluster = updated_cloud; } return output; diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index 17ed9a447cdb8..53d77e8d1c42c 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -6,6 +6,7 @@ head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" trt_precision: fp16 + cloud_capacity: 2000000 post_process_params: # post-process params circle_nms_dist_threshold: 0.5 diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 17ed9a447cdb8..53d77e8d1c42c 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -6,6 +6,7 @@ head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" trt_precision: fp16 + cloud_capacity: 2000000 post_process_params: # post-process params circle_nms_dist_threshold: 0.5 diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index 19fdbe7a8b9c2..7b560cf46e2e3 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp @@ -24,14 +24,16 @@ class CenterPointConfig { public: explicit CenterPointConfig( - const std::size_t class_size, const float point_feature_size, const std::size_t max_voxel_size, - const std::vector & point_cloud_range, const std::vector & voxel_size, - const std::size_t downsample_factor, const std::size_t encoder_in_feature_size, - const float score_threshold, const float circle_nms_dist_threshold, - const std::vector yaw_norm_thresholds, const bool has_variance) + const std::size_t class_size, const float point_feature_size, const std::size_t cloud_capacity, + const std::size_t max_voxel_size, const std::vector & point_cloud_range, + const std::vector & voxel_size, const std::size_t downsample_factor, + const std::size_t encoder_in_feature_size, const float score_threshold, + const float circle_nms_dist_threshold, const std::vector yaw_norm_thresholds, + const bool has_variance) { class_size_ = class_size; point_feature_size_ = point_feature_size; + cloud_capacity_ = cloud_capacity; max_voxel_size_ = max_voxel_size; if (point_cloud_range.size() == 6) { range_min_x_ = static_cast(point_cloud_range[0]); @@ -85,6 +87,7 @@ class CenterPointConfig }; // input params + std::size_t cloud_capacity_{}; std::size_t class_size_{3}; const std::size_t point_dim_size_{3}; // x, y and z std::size_t point_feature_size_{4}; // x, y, z and time-lag diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp index 55157f70fcfc3..52ae86951c7cf 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp @@ -31,8 +31,6 @@ namespace centerpoint { -static constexpr size_t CAPACITY_POINT = 1000000; - class NetworkParam { public: diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 6f379167b0e71..5040028d1bd94 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -85,7 +85,7 @@ void CenterPointTRT::initPtr() mask_size_ = config_.grid_size_x_ * config_.grid_size_y_; // host - points_.resize(CAPACITY_POINT * config_.point_feature_size_); + points_.resize(config_.cloud_capacity_ * config_.point_feature_size_); // device voxels_d_ = cuda::make_unique(voxels_size_); @@ -100,7 +100,7 @@ void CenterPointTRT::initPtr() head_out_dim_d_ = cuda::make_unique(grid_xy_size * config_.head_out_dim_size_); head_out_rot_d_ = cuda::make_unique(grid_xy_size * config_.head_out_rot_size_); head_out_vel_d_ = cuda::make_unique(grid_xy_size * config_.head_out_vel_size_); - points_d_ = cuda::make_unique(CAPACITY_POINT * config_.point_feature_size_); + points_d_ = cuda::make_unique(config_.cloud_capacity_ * config_.point_feature_size_); voxels_buffer_d_ = cuda::make_unique(voxels_buffer_size_); mask_d_ = cuda::make_unique(mask_size_); num_voxels_d_ = cuda::make_unique(1); diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index e90474fa07327..8e91c3b0b36cc 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -61,7 +61,7 @@ std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t s float time_lag = static_cast( pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds()); - if (point_counter + sweep_num_points > CAPACITY_POINT) { + if (point_counter + sweep_num_points > config_.cloud_capacity_) { RCLCPP_WARN_STREAM( rclcpp::get_logger("lidar_centerpoint"), "Requested number of points exceeds the maximum capacity. Current points = " diff --git a/perception/lidar_centerpoint/schema/centerpoint.schemal.json b/perception/lidar_centerpoint/schema/centerpoint.schemal.json index b11c115cc2466..c3268f0e90d9b 100644 --- a/perception/lidar_centerpoint/schema/centerpoint.schemal.json +++ b/perception/lidar_centerpoint/schema/centerpoint.schemal.json @@ -12,6 +12,12 @@ "default": "fp16", "enum": ["fp32", "fp16"] }, + "cloud_capacity": { + "type": "integer", + "description": "Capacity of the point cloud buffer (should be set to at least the maximum theoretical number of points).", + "default": 2000000, + "minimum": 1 + }, "post_process_params": { "type": "object", "properties": { diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 8bf62e3e0168b..53554d0a3becf 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -50,6 +50,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const int densification_num_past_frames = this->declare_parameter("densification_params.num_past_frames"); const std::string trt_precision = this->declare_parameter("trt_precision"); + const std::size_t cloud_capacity = this->declare_parameter("cloud_capacity"); const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); const std::string encoder_engine_path = this->declare_parameter("encoder_engine_path"); @@ -104,9 +105,9 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti "The size of voxel_size != 3: use the default parameters."); } CenterPointConfig config( - class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, - downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, - yaw_norm_thresholds, has_variance_); + class_names_.size(), point_feature_size, cloud_capacity, max_voxel_size, point_cloud_range, + voxel_size, downsample_factor, encoder_in_feature_size, score_threshold, + circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); diff --git a/perception/lidar_transfusion/CMakeLists.txt b/perception/lidar_transfusion/CMakeLists.txt index c0e2d85f27e62..15c89beb15d3e 100644 --- a/perception/lidar_transfusion/CMakeLists.txt +++ b/perception/lidar_transfusion/CMakeLists.txt @@ -134,9 +134,66 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE lidar_transfusion_node ) + install( + TARGETS transfusion_cuda_lib + DESTINATION lib + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_detection_class_remapper + test/test_detection_class_remapper.cpp + ) + ament_auto_add_gtest(test_ros_utils + test/test_ros_utils.cpp + ) + ament_auto_add_gtest(test_nms + test/test_nms.cpp + ) + + # Temporary disabled, tracked by: + # https://github.com/autowarefoundation/autoware.universe/issues/7724 + # ament_auto_add_gtest(test_voxel_generator + # test/test_voxel_generator.cpp + # ) + + # # preprocess kernel test + # add_executable(test_preprocess_kernel + # test/test_preprocess_kernel.cpp + # ) + # target_include_directories(test_preprocess_kernel PUBLIC + # ${test_preprocess_kernel_SOURCE_DIR} + # ) + # target_link_libraries(test_preprocess_kernel + # transfusion_cuda_lib + # gtest + # gtest_main + # ) + # ament_add_test(test_preprocess_kernel + # GENERATE_RESULT_FOR_RETURN_CODE_ZERO + # COMMAND "$" + # ) + + # # postprocess kernel test + # add_executable(test_postprocess_kernel + # test/test_postprocess_kernel.cpp + # ) + # target_include_directories(test_postprocess_kernel PUBLIC + # ${test_postprocess_kernel_SOURCE_DIR} + # ) + # target_link_libraries(test_postprocess_kernel + # transfusion_cuda_lib + # gtest + # gtest_main + # ) + # ament_add_test(test_postprocess_kernel + # GENERATE_RESULT_FOR_RETURN_CODE_ZERO + # COMMAND "$" + # ) + endif() ament_auto_package( diff --git a/perception/lidar_transfusion/README.md b/perception/lidar_transfusion/README.md index 7974714720c32..6745cc1f7d219 100644 --- a/perception/lidar_transfusion/README.md +++ b/perception/lidar_transfusion/README.md @@ -59,13 +59,31 @@ ros2 launch lidar_transfusion lidar_transfusion.launch.xml log_level:=debug ## Assumptions / Known limits +This library operates on raw cloud data (bytes). It is assumed that the input pointcloud message has following format: + +```python +[ + sensor_msgs.msg.PointField(name='x', offset=0, datatype=7, count=1), + sensor_msgs.msg.PointField(name='y', offset=4, datatype=7, count=1), + sensor_msgs.msg.PointField(name='z', offset=8, datatype=7, count=1), + sensor_msgs.msg.PointField(name='intensity', offset=12, datatype=2, count=1) +] +``` + +This input may consist of other fields as well - shown format is required minimum. +For debug purposes, you can validate your pointcloud topic using simple command: + +```bash +ros2 topic echo --field fields +``` + ## Trained Models You can download the onnx format of trained models by clicking on the links below. -- TransFusion: [transfusion.onnx](https://awf.ml.dev.web.auto/perception/models/transfusion/v1/transfusion.onnx) +- TransFusion: [transfusion.onnx](https://awf.ml.dev.web.auto/perception/models/transfusion/t4xx1_90m/v2/transfusion.onnx) -The model was trained in TIER IV's internal database (~11k lidar frames) for 20 epochs. +The model was trained in TIER IV's internal database (~11k lidar frames) for 50 epochs. ### Changelog diff --git a/perception/lidar_transfusion/config/transfusion.param.yaml b/perception/lidar_transfusion/config/transfusion.param.yaml index feabe359caf1f..af1f63c335501 100644 --- a/perception/lidar_transfusion/config/transfusion.param.yaml +++ b/perception/lidar_transfusion/config/transfusion.param.yaml @@ -3,9 +3,11 @@ # network class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] trt_precision: fp16 + cloud_capacity: 2000000 voxels_num: [5000, 30000, 60000] # [min, opt, max] - point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] # [x_min, y_min, z_min, x_max, y_max, z_max] - voxel_size: [0.3, 0.3, 8.0] # [x, y, z] + point_cloud_range: [-92.16, -92.16, -3.0, 92.16, 92.16, 7.0] # [x_min, y_min, z_min, x_max, y_max, z_max] + voxel_size: [0.24, 0.24, 10.0] # [x, y, z] + num_proposals: 500 onnx_path: "$(var model_path)/transfusion.onnx" engine_path: "$(var model_path)/transfusion.engine" # pre-process params @@ -17,4 +19,4 @@ iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] # refers to the class_names - score_threshold: 0.2 + score_threshold: 0.1 diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp index 25095f4dc9c0b..6ac0a6544389f 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp @@ -51,7 +51,7 @@ class DensificationParam struct PointCloudWithTransform { - cuda::unique_ptr points_d{nullptr}; + cuda::unique_ptr data_d{nullptr}; std_msgs::msg::Header header; size_t num_points{0}; Eigen::Affine3f affine_past2world; diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp index c571990d84b51..592f09c2d288a 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp @@ -56,12 +56,8 @@ class PreprocessCuda unsigned int * mask, float * voxels, unsigned int * pillar_num, float * voxel_features, unsigned int * voxel_num, unsigned int * voxel_idxs); - // cudaError_t generateVoxelsInput_launch( - // uint8_t * cloud_data, CloudInfo & cloud_info, unsigned int points_agg, unsigned int - // points_size, float time_lag, float * affine_transform, float * points); - cudaError_t generateSweepPoints_launch( - const float * input_points, size_t points_size, int input_point_step, float time_lag, + const uint8_t * input_data, size_t points_size, int input_point_step, float time_lag, const float * transform, float * output_points); private: diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp index 3e3660e238473..f0d253ee28755 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp @@ -18,8 +18,8 @@ #include "lidar_transfusion/cuda_utils.hpp" #include "lidar_transfusion/preprocess/pointcloud_densification.hpp" #include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/ros_utils.hpp" #include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -27,6 +27,8 @@ #include #endif +#include + #include #include @@ -36,8 +38,8 @@ namespace lidar_transfusion { -constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix -constexpr size_t MAX_CLOUD_STEP_SIZE = 35; // PointXYZIRCADT +constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix +constexpr size_t MAX_CLOUD_STEP_SIZE = sizeof(autoware_point_types::PointXYZIRCAEDT); class VoxelGenerator { diff --git a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp index f013a02404a29..cbfc4e87b1610 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp @@ -17,16 +17,84 @@ #include "lidar_transfusion/utils.hpp" +#include + #include #include #include #include +#include +#include #include #include +#define CHECK_OFFSET(offset, structure, field) \ + static_assert( \ + offsetof(structure, field) == offset, \ + "Offset of " #field " in " #structure " does not match expected offset.") +#define CHECK_TYPE(type, structure, field) \ + static_assert( \ + std::is_same::value, \ + "Type of " #field " in " #structure " does not match expected type.") +#define CHECK_FIELD(offset, type, structure, field) \ + CHECK_OFFSET(offset, structure, field); \ + CHECK_TYPE(type, structure, field) + namespace lidar_transfusion { +using sensor_msgs::msg::PointField; + +CHECK_FIELD(0, float, autoware_point_types::PointXYZIRCAEDT, x); +CHECK_FIELD(4, float, autoware_point_types::PointXYZIRCAEDT, y); +CHECK_FIELD(8, float, autoware_point_types::PointXYZIRCAEDT, z); +CHECK_FIELD(12, uint8_t, autoware_point_types::PointXYZIRCAEDT, intensity); + +struct CloudInfo +{ + uint32_t x_offset{0}; + uint32_t y_offset{sizeof(float)}; + uint32_t z_offset{sizeof(float) * 2}; + uint32_t intensity_offset{sizeof(float) * 3}; + uint8_t x_datatype{PointField::FLOAT32}; + uint8_t y_datatype{PointField::FLOAT32}; + uint8_t z_datatype{PointField::FLOAT32}; + uint8_t intensity_datatype{PointField::UINT8}; + uint8_t x_count{1}; + uint8_t y_count{1}; + uint8_t z_count{1}; + uint8_t intensity_count{1}; + uint32_t point_step{sizeof(autoware_point_types::PointXYZIRCAEDT)}; + bool is_bigendian{false}; + + bool operator!=(const CloudInfo & rhs) const + { + return x_offset != rhs.x_offset || y_offset != rhs.y_offset || z_offset != rhs.z_offset || + intensity_offset != rhs.intensity_offset || x_datatype != rhs.x_datatype || + y_datatype != rhs.y_datatype || z_datatype != rhs.z_datatype || + intensity_datatype != rhs.intensity_datatype || x_count != rhs.x_count || + y_count != rhs.y_count || z_count != rhs.z_count || + intensity_count != rhs.intensity_count || is_bigendian != rhs.is_bigendian; + } + + friend std::ostream & operator<<(std::ostream & os, const CloudInfo & info) + { + os << "x_offset: " << static_cast(info.x_offset) << std::endl; + os << "y_offset: " << static_cast(info.y_offset) << std::endl; + os << "z_offset: " << static_cast(info.z_offset) << std::endl; + os << "intensity_offset: " << static_cast(info.intensity_offset) << std::endl; + os << "x_datatype: " << static_cast(info.x_datatype) << std::endl; + os << "y_datatype: " << static_cast(info.y_datatype) << std::endl; + os << "z_datatype: " << static_cast(info.z_datatype) << std::endl; + os << "intensity_datatype: " << static_cast(info.intensity_datatype) << std::endl; + os << "x_count: " << static_cast(info.x_count) << std::endl; + os << "y_count: " << static_cast(info.y_count) << std::endl; + os << "z_count: " << static_cast(info.z_count) << std::endl; + os << "intensity_count: " << static_cast(info.intensity_count) << std::endl; + os << "is_bigendian: " << static_cast(info.is_bigendian) << std::endl; + return os; + } +}; void box3DToDetectedObject( const Box3D & box3d, const std::vector & class_names, diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp index 31976de56a9da..0d0148d2f6c17 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp @@ -25,10 +25,13 @@ class TransfusionConfig { public: TransfusionConfig( - const std::vector & voxels_num, const std::vector & point_cloud_range, - const std::vector & voxel_size, const float circle_nms_dist_threshold, + const std::size_t cloud_capacity, const std::vector & voxels_num, + const std::vector & point_cloud_range, const std::vector & voxel_size, + const std::size_t num_proposals, const float circle_nms_dist_threshold, const std::vector & yaw_norm_thresholds, const float score_threshold) { + cloud_capacity_ = cloud_capacity; + if (voxels_num.size() == 3) { max_voxels_ = voxels_num[2]; @@ -61,6 +64,9 @@ class TransfusionConfig voxel_y_size_ = static_cast(voxel_size[1]); voxel_z_size_ = static_cast(voxel_size[2]); } + if (num_proposals > 0) { + num_proposals_ = num_proposals; + } if (score_threshold > 0.0) { score_threshold_ = score_threshold; } @@ -76,10 +82,13 @@ class TransfusionConfig grid_x_size_ = static_cast((max_x_range_ - min_x_range_) / voxel_x_size_); grid_y_size_ = static_cast((max_y_range_ - min_y_range_) / voxel_y_size_); grid_z_size_ = static_cast((max_z_range_ - min_z_range_) / voxel_z_size_); + + feature_x_size_ = grid_x_size_ / out_size_factor_; + feature_y_size_ = grid_y_size_ / out_size_factor_; } ///// INPUT PARAMETERS ///// - const std::size_t cloud_capacity_{1000000}; + std::size_t cloud_capacity_{}; ///// KERNEL PARAMETERS ///// const std::size_t threads_for_voxel_{256}; // threads number for a block const std::size_t points_per_voxel_{20}; @@ -107,7 +116,7 @@ class TransfusionConfig const std::size_t out_size_factor_{4}; const std::size_t max_num_points_per_pillar_{points_per_voxel_}; const std::size_t num_point_values_{4}; - const std::size_t num_proposals_{200}; + std::size_t num_proposals_{200}; // the number of feature maps for pillar scatter const std::size_t num_feature_scatter_{pillar_feature_size_}; // the score threshold for classification diff --git a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp index e73fe7f055953..cc40e55851738 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp @@ -36,52 +36,6 @@ struct Box3D float yaw; }; -struct CloudInfo -{ - uint32_t x_offset; - uint32_t y_offset; - uint32_t z_offset; - uint32_t intensity_offset; - uint8_t x_datatype; - uint8_t y_datatype; - uint8_t z_datatype; - uint8_t intensity_datatype; - uint8_t x_count; - uint8_t y_count; - uint8_t z_count; - uint8_t intensity_count; - uint32_t point_step; - bool is_bigendian; - - CloudInfo() - : x_offset(0), - y_offset(4), - z_offset(8), - intensity_offset(12), - x_datatype(7), - y_datatype(7), - z_datatype(7), - intensity_datatype(7), - x_count(1), - y_count(1), - z_count(1), - intensity_count(1), - point_step(16), - is_bigendian(false) - { - } - - bool operator!=(const CloudInfo & rhs) const - { - return x_offset != rhs.x_offset || y_offset != rhs.y_offset || z_offset != rhs.z_offset || - intensity_offset != rhs.intensity_offset || x_datatype != rhs.x_datatype || - y_datatype != rhs.y_datatype || z_datatype != rhs.z_datatype || - intensity_datatype != rhs.intensity_datatype || x_count != rhs.x_count || - y_count != rhs.y_count || z_count != rhs.z_count || - intensity_count != rhs.intensity_count || is_bigendian != rhs.is_bigendian; - } -}; - enum NetworkIO { voxels = 0, num_points, coors, cls_score, dir_pred, bbox_pred, ENUM_SIZE }; // cspell: ignore divup diff --git a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp index 774b3a05d71c1..c13423f2d24d8 100644 --- a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp +++ b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp @@ -93,16 +93,15 @@ void PointCloudDensification::enqueue( affine_world2current_ = affine_world2current; current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); - assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 1); - auto points_d = cuda::make_unique( - sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float)); + auto data_d = cuda::make_unique( + sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(uint8_t)); CHECK_CUDA_ERROR(cudaMemcpyAsync( - points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, + data_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, cudaMemcpyHostToDevice, stream_)); PointCloudWithTransform pointcloud = { - std::move(points_d), msg.header, msg.width * msg.height, affine_world2current.inverse()}; + std::move(data_d), msg.header, msg.width * msg.height, affine_world2current.inverse()}; pointcloud_cache_.push_front(std::move(pointcloud)); } diff --git a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu index b8f9ae998fd4f..48bb4eb9332a8 100644 --- a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -31,6 +31,8 @@ #include "lidar_transfusion/cuda_utils.hpp" #include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include + namespace lidar_transfusion { @@ -99,9 +101,12 @@ __global__ void generateVoxels_random_kernel( cudaError_t PreprocessCuda::generateVoxels_random_launch( float * points, unsigned int points_size, unsigned int * mask, float * voxels) { - int threadNum = config_.threads_for_voxel_; - dim3 blocks((points_size + threadNum - 1) / threadNum); - dim3 threads(threadNum); + if (points_size == 0) { + return cudaGetLastError(); + } + dim3 blocks(divup(points_size, config_.threads_for_voxel_)); + dim3 threads(config_.threads_for_voxel_); + generateVoxels_random_kernel<<>>( points, points_size, config_.min_x_range_, config_.max_x_range_, config_.min_y_range_, config_.max_y_range_, config_.min_z_range_, config_.max_z_range_, config_.voxel_x_size_, @@ -165,40 +170,48 @@ cudaError_t PreprocessCuda::generateBaseFeatures_launch( } __global__ void generateSweepPoints_kernel( - const float * input_points, size_t points_size, int input_point_step, float time_lag, + const uint8_t * input_data, size_t points_size, int input_point_step, float time_lag, const float * transform_array, int num_features, float * output_points) { int point_idx = blockIdx.x * blockDim.x + threadIdx.x; if (point_idx >= points_size) return; - const float input_x = input_points[point_idx * input_point_step + 0]; - const float input_y = input_points[point_idx * input_point_step + 1]; - const float input_z = input_points[point_idx * input_point_step + 2]; - const float intensity = input_points[point_idx * input_point_step + 3]; - - output_points[point_idx * num_features] = transform_array[0] * input_x + - transform_array[4] * input_y + - transform_array[8] * input_z + transform_array[12]; - output_points[point_idx * num_features + 1] = transform_array[1] * input_x + - transform_array[5] * input_y + - transform_array[9] * input_z + transform_array[13]; - output_points[point_idx * num_features + 2] = transform_array[2] * input_x + - transform_array[6] * input_y + - transform_array[10] * input_z + transform_array[14]; - output_points[point_idx * num_features + 3] = intensity; + union { + uint32_t raw{0}; + float value; + } input_x, input_y, input_z; + +#pragma unroll + for (int i = 0; i < 4; i++) { // 4 bytes for float32 + input_x.raw |= input_data[point_idx * input_point_step + i] << i * 8; + input_y.raw |= input_data[point_idx * input_point_step + i + 4] << i * 8; + input_z.raw |= input_data[point_idx * input_point_step + i + 8] << i * 8; + } + + float input_intensity = static_cast(input_data[point_idx * input_point_step + 12]); + + output_points[point_idx * num_features] = + transform_array[0] * input_x.value + transform_array[4] * input_y.value + + transform_array[8] * input_z.value + transform_array[12]; + output_points[point_idx * num_features + 1] = + transform_array[1] * input_x.value + transform_array[5] * input_y.value + + transform_array[9] * input_z.value + transform_array[13]; + output_points[point_idx * num_features + 2] = + transform_array[2] * input_x.value + transform_array[6] * input_y.value + + transform_array[10] * input_z.value + transform_array[14]; + output_points[point_idx * num_features + 3] = input_intensity; output_points[point_idx * num_features + 4] = time_lag; } cudaError_t PreprocessCuda::generateSweepPoints_launch( - const float * input_points, size_t points_size, int input_point_step, float time_lag, + const uint8_t * input_data, size_t points_size, int input_point_step, float time_lag, const float * transform_array, float * output_points) { - int threadNum = config_.threads_for_voxel_; - dim3 blocks((points_size + threadNum - 1) / threadNum); - dim3 threads(threadNum); + dim3 blocks(divup(points_size, config_.threads_for_voxel_)); + dim3 threads(config_.threads_for_voxel_); generateSweepPoints_kernel<<>>( - input_points, points_size, input_point_step, time_lag, transform_array, + input_data, points_size, input_point_step, time_lag, transform_array, config_.num_point_feature_size_, output_points); cudaError_t err = cudaGetLastError(); diff --git a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp index 7072e8491af66..cb8bac984aef3 100644 --- a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp @@ -23,25 +23,6 @@ namespace lidar_transfusion { -std::ostream & operator<<(std::ostream & os, const CloudInfo & info) -{ - os << "x_offset: " << static_cast(info.x_offset) << std::endl; - os << "y_offset: " << static_cast(info.y_offset) << std::endl; - os << "z_offset: " << static_cast(info.z_offset) << std::endl; - os << "intensity_offset: " << static_cast(info.intensity_offset) << std::endl; - os << "x_datatype: " << static_cast(info.x_datatype) << std::endl; - os << "y_datatype: " << static_cast(info.y_datatype) << std::endl; - os << "z_datatype: " << static_cast(info.z_datatype) << std::endl; - os << "intensity_datatype: " << static_cast(info.intensity_datatype) << std::endl; - os << "x_count: " << static_cast(info.x_count) << std::endl; - os << "y_count: " << static_cast(info.y_count) << std::endl; - os << "z_count: " << static_cast(info.z_count) << std::endl; - os << "intensity_count: " << static_cast(info.intensity_count) << std::endl; - os << "point_step: " << static_cast(info.point_step) << std::endl; - os << "is_bigendian: " << static_cast(info.is_bigendian) << std::endl; - return os; -} - VoxelGenerator::VoxelGenerator( const DensificationParam & densification_param, const TransfusionConfig & config, cudaStream_t & stream) @@ -106,8 +87,8 @@ std::size_t VoxelGenerator::generateSweepPoints( CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); pre_ptr_->generateSweepPoints_launch( - pc_cache_iter->points_d.get(), sweep_num_points, cloud_info_.point_step / sizeof(float), - time_lag, affine_past2current_d_.get(), points_d.get() + shift); + pc_cache_iter->data_d.get(), sweep_num_points, cloud_info_.point_step, time_lag, + affine_past2current_d_.get(), points_d.get() + shift); point_counter += sweep_num_points; } diff --git a/perception/lidar_transfusion/package.xml b/perception/lidar_transfusion/package.xml index ffbe091b998e5..3b495025b1c34 100644 --- a/perception/lidar_transfusion/package.xml +++ b/perception/lidar_transfusion/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_perception_msgs + autoware_point_types autoware_universe_utils launch_ros object_recognition_utils diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/lidar_transfusion/schema/transfusion.schema.json index 41d8d887236a8..b37de5a97e7c0 100644 --- a/perception/lidar_transfusion/schema/transfusion.schema.json +++ b/perception/lidar_transfusion/schema/transfusion.schema.json @@ -21,6 +21,12 @@ "default": "fp16", "enum": ["fp16", "fp32"] }, + "cloud_capacity": { + "type": "integer", + "description": "Capacity of the point cloud buffer (should be set to at least the maximum theoretical number of points).", + "default": 2000000, + "minimum": 1 + }, "voxels_num": { "type": "array", "items": { @@ -61,6 +67,12 @@ "default": "$(var model_path)/transfusion.engine", "pattern": "\\.engine$" }, + "num_proposals": { + "type": "integer", + "description": "Number of proposals at TransHead.", + "default": 500, + "minimum": 1 + }, "down_sample_factor": { "type": "integer", "description": "A scale factor of downsampling points", diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp index e3ea6b3780de8..a07e385208748 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -27,10 +27,14 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) class_names_ = this->declare_parameter>("class_names", descriptor); const std::string trt_precision = this->declare_parameter("trt_precision", descriptor); + const std::size_t cloud_capacity = + this->declare_parameter("cloud_capacity", descriptor); const auto voxels_num = this->declare_parameter>("voxels_num", descriptor); const auto point_cloud_range = this->declare_parameter>("point_cloud_range", descriptor); const auto voxel_size = this->declare_parameter>("voxel_size", descriptor); + const std::size_t num_proposals = + this->declare_parameter("num_proposals", descriptor); const std::string onnx_path = this->declare_parameter("onnx_path", descriptor); const std::string engine_path = this->declare_parameter("engine_path", descriptor); @@ -73,8 +77,8 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); TransfusionConfig config( - voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, - score_threshold); + cloud_capacity, voxels_num, point_cloud_range, voxel_size, num_proposals, + circle_nms_dist_threshold, yaw_norm_thresholds, score_threshold); const auto allow_remapping_by_area_matrix = this->declare_parameter>("allow_remapping_by_area_matrix", descriptor); diff --git a/perception/lidar_transfusion/test/test_detection_class_remapper.cpp b/perception/lidar_transfusion/test/test_detection_class_remapper.cpp new file mode 100644 index 0000000000000..6e7f896e44d2c --- /dev/null +++ b/perception/lidar_transfusion/test/test_detection_class_remapper.cpp @@ -0,0 +1,92 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +TEST(DetectionClassRemapperTest, MapClasses) +{ + lidar_transfusion::DetectionClassRemapper remapper; + + // Set up the parameters for the remapper + // Labels: CAR, TRUCK, TRAILER + std::vector allow_remapping_by_area_matrix = { + 0, 0, 0, // CAR cannot be remapped + 0, 0, 1, // TRUCK can be remapped to TRAILER + 0, 1, 0 // TRAILER can be remapped to TRUCK + }; + std::vector min_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0}; + std::vector max_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 999.0, 0.0, 10.0, 0.0}; + + remapper.setParameters(allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + // Create a DetectedObjects message with some objects + autoware_perception_msgs::msg::DetectedObjects msg; + + // CAR with area 4.0, which is out of the range for remapping + autoware_perception_msgs::msg::DetectedObject obj1; + autoware_perception_msgs::msg::ObjectClassification obj1_classification; + obj1.shape.dimensions.x = 2.0; + obj1.shape.dimensions.y = 2.0; + obj1_classification.label = 0; + obj1_classification.probability = 1.0; + obj1.classification = {obj1_classification}; + msg.objects.push_back(obj1); + + // TRUCK with area 16.0, which is in the range for remapping to TRAILER + autoware_perception_msgs::msg::DetectedObject obj2; + autoware_perception_msgs::msg::ObjectClassification obj2_classification; + obj2.shape.dimensions.x = 8.0; + obj2.shape.dimensions.y = 2.0; + obj2_classification.label = 1; + obj2_classification.probability = 1.0; + obj2.classification = {obj2_classification}; + msg.objects.push_back(obj2); + + // TRAILER with area 9.0, which is in the range for remapping to TRUCK + autoware_perception_msgs::msg::DetectedObject obj3; + autoware_perception_msgs::msg::ObjectClassification obj3_classification; + obj3.shape.dimensions.x = 3.0; + obj3.shape.dimensions.y = 3.0; + obj3_classification.label = 2; + obj3_classification.probability = 1.0; + obj3.classification = {obj3_classification}; + msg.objects.push_back(obj3); + + // TRAILER with area 12.0, which is out of the range for remapping + autoware_perception_msgs::msg::DetectedObject obj4; + autoware_perception_msgs::msg::ObjectClassification obj4_classification; + obj4.shape.dimensions.x = 4.0; + obj4.shape.dimensions.y = 3.0; + obj4_classification.label = 2; + obj4_classification.probability = 1.0; + obj4.classification = {obj4_classification}; + msg.objects.push_back(obj4); + + // Call the mapClasses function + remapper.mapClasses(msg); + + // Check the remapped labels + EXPECT_EQ(msg.objects[0].classification[0].label, 0); + EXPECT_EQ(msg.objects[1].classification[0].label, 2); + EXPECT_EQ(msg.objects[2].classification[0].label, 1); + EXPECT_EQ(msg.objects[3].classification[0].label, 2); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_nms.cpp b/perception/lidar_transfusion/test/test_nms.cpp new file mode 100644 index 0000000000000..b6f0ec8c31684 --- /dev/null +++ b/perception/lidar_transfusion/test/test_nms.cpp @@ -0,0 +1,121 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" + +#include + +TEST(NonMaximumSuppressionTest, Apply) +{ + lidar_transfusion::NonMaximumSuppression nms; + lidar_transfusion::NMSParams params; + params.search_distance_2d_ = 1.0; + params.iou_threshold_ = 0.2; + params.nms_type_ = lidar_transfusion::NMS_TYPE::IoU_BEV; + params.target_class_names_ = {"CAR"}; + nms.setParameters(params); + + std::vector input_objects(4); + + // Object 1 + autoware_perception_msgs::msg::ObjectClassification obj1_classification; + obj1_classification.label = 0; // Assuming "car" has label 0 + obj1_classification.probability = 1.0; + input_objects[0].classification = {obj1_classification}; // Assuming "car" has label 0 + input_objects[0].kinematics.pose_with_covariance.pose.position.x = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.position.y = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[0].shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[0].shape.dimensions.x = 4.0; + input_objects[0].shape.dimensions.y = 2.0; + + // Object 2 (overlaps with Object 1) + autoware_perception_msgs::msg::ObjectClassification obj2_classification; + obj2_classification.label = 0; // Assuming "car" has label 0 + obj2_classification.probability = 1.0; + input_objects[1].classification = {obj2_classification}; // Assuming "car" has label 0 + input_objects[1].kinematics.pose_with_covariance.pose.position.x = 0.5; + input_objects[1].kinematics.pose_with_covariance.pose.position.y = 0.5; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[1].shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[1].shape.dimensions.x = 4.0; + input_objects[1].shape.dimensions.y = 2.0; + + // Object 3 + autoware_perception_msgs::msg::ObjectClassification obj3_classification; + obj3_classification.label = 0; // Assuming "car" has label 0 + obj3_classification.probability = 1.0; + input_objects[2].classification = {obj3_classification}; // Assuming "car" has label 0 + input_objects[2].kinematics.pose_with_covariance.pose.position.x = 5.0; + input_objects[2].kinematics.pose_with_covariance.pose.position.y = 5.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[2].shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[2].shape.dimensions.x = 4.0; + input_objects[2].shape.dimensions.y = 2.0; + + // Object 4 (different class) + autoware_perception_msgs::msg::ObjectClassification obj4_classification; + obj4_classification.label = 1; // Assuming "pedestrian" has label 1 + obj4_classification.probability = 1.0; + input_objects[3].classification = {obj4_classification}; // Assuming "pedestrian" has label 1 + input_objects[3].kinematics.pose_with_covariance.pose.position.x = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.position.y = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[3].shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[3].shape.dimensions.x = 0.5; + input_objects[3].shape.dimensions.y = 0.5; + + std::vector output_objects = nms.apply(input_objects); + + // Assert the expected number of output objects + EXPECT_EQ(output_objects.size(), 3); + + // Assert that input_objects[2] is included in the output_objects + bool is_input_object_2_included = false; + for (const auto & output_object : output_objects) { + if (output_object == input_objects[2]) { + is_input_object_2_included = true; + break; + } + } + EXPECT_TRUE(is_input_object_2_included); + + // Assert that input_objects[3] is included in the output_objects + bool is_input_object_3_included = false; + for (const auto & output_object : output_objects) { + if (output_object == input_objects[3]) { + is_input_object_3_included = true; + break; + } + } + EXPECT_TRUE(is_input_object_3_included); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_postprocess_kernel.cpp b/perception/lidar_transfusion/test/test_postprocess_kernel.cpp new file mode 100644 index 0000000000000..1e01db5a8b3d7 --- /dev/null +++ b/perception/lidar_transfusion/test/test_postprocess_kernel.cpp @@ -0,0 +1,299 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_postprocess_kernel.hpp" + +#include + +#include +#include + +namespace lidar_transfusion +{ + +void PostprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + auto voxels_num = std::vector{1, 3, 5}; + auto point_cloud_range = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; + auto voxel_size = std::vector{0.3, 0.3, 8.0}; + auto score_threshold = 0.2f; + auto circle_nms_dist_threshold = 0.5f; + auto yaw_norm_thresholds = std::vector{0.5, 0.5, 0.5}; + config_ptr_ = std::make_unique( + voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, + score_threshold); + post_ptr_ = std::make_unique(*config_ptr_, stream_); + + cls_size_ = config_ptr_->num_proposals_ * config_ptr_->num_classes_; + box_size_ = config_ptr_->num_proposals_ * config_ptr_->num_box_values_; + dir_cls_size_ = config_ptr_->num_proposals_ * 2; // x, y + + cls_output_d_ = cuda::make_unique(cls_size_); + box_output_d_ = cuda::make_unique(box_size_); + dir_cls_output_d_ = cuda::make_unique(dir_cls_size_); + + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); +} + +void PostprocessKernelTest::TearDown() +{ +} + +TEST_F(PostprocessKernelTest, EmptyTensorTest) +{ + std::vector det_boxes3d; + + CHECK_CUDA_ERROR(post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_)); + + EXPECT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, SingleDetectionTest) +{ + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + std::vector det_boxes3d; + unsigned int det_idx = 42; + unsigned int cls_idx = 2; + + constexpr float detection_score = 1.f; + constexpr float detection_x = 50.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + const float detection_raw_x = (detection_x - config_ptr_->min_x_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_x_size_); + const float detection_raw_y = (detection_y - config_ptr_->min_y_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_y_size_); + const float detection_raw_z = detection_z; + + constexpr float detection_w = 3.2f; + constexpr float detection_l = 9.3f; + constexpr float detection_h = 1.7f; + constexpr float detection_log_w = std::log(detection_w); + constexpr float detection_log_l = std::log(detection_l); + constexpr float detection_log_h = std::log(detection_h); + + constexpr float detection_yaw = M_PI_4; + constexpr float detection_yaw_sin = std::sin(detection_yaw); + constexpr float detection_yaw_cos = std::cos(detection_yaw); + + // Set the values in the cuda tensor + cudaMemcpy( + &cls_output_d_[cls_idx * config_ptr_->num_proposals_ + det_idx], &detection_score, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx], &detection_yaw_sin, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx + config_ptr_->num_proposals_], &detection_yaw_cos, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy(&box_output_d_[det_idx], &detection_raw_x, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + config_ptr_->num_proposals_], &detection_raw_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 2 * config_ptr_->num_proposals_], &detection_raw_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 3 * config_ptr_->num_proposals_], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 4 * config_ptr_->num_proposals_], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 5 * config_ptr_->num_proposals_], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code1 = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code1); + + // Extract the boxes + auto code2 = post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + ASSERT_EQ(cudaSuccess, code2); + ASSERT_EQ(1, det_boxes3d.size()); + + const auto det_box3d = det_boxes3d[0]; + EXPECT_EQ(cls_idx, det_box3d.label); + EXPECT_EQ(detection_score, det_box3d.score); + EXPECT_EQ(detection_x, det_box3d.x); + EXPECT_EQ(detection_y, det_box3d.y); + EXPECT_EQ(detection_z, det_box3d.z); + EXPECT_NEAR(detection_w, det_box3d.width, 1e-6); + EXPECT_NEAR(detection_l, det_box3d.length, 1e-6); + EXPECT_NEAR(detection_h, det_box3d.height, 1e-6); + EXPECT_EQ(detection_yaw, det_box3d.yaw); +} + +TEST_F(PostprocessKernelTest, InvalidYawTest) +{ + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + std::vector det_boxes3d; + unsigned int det_idx = 42; + unsigned int cls_idx = 2; + + constexpr float detection_score = 1.f; + constexpr float detection_x = 50.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + const float detection_raw_x = (detection_x - config_ptr_->min_x_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_x_size_); + const float detection_raw_y = (detection_y - config_ptr_->min_y_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_y_size_); + const float detection_raw_z = detection_z; + + constexpr float detection_w = 3.2f; + constexpr float detection_l = 9.3f; + constexpr float detection_h = 1.7f; + constexpr float detection_log_w = std::log(detection_w); + constexpr float detection_log_l = std::log(detection_l); + constexpr float detection_log_h = std::log(detection_h); + + constexpr float detection_yaw_sin = 0.f; + constexpr float detection_yaw_cos = 0.2f; + + // Set the values in the cuda tensor + cudaMemcpy( + &cls_output_d_[cls_idx * config_ptr_->num_proposals_ + det_idx], &detection_score, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx], &detection_yaw_sin, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx + config_ptr_->num_proposals_], &detection_yaw_cos, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy(&box_output_d_[det_idx], &detection_raw_x, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + config_ptr_->num_proposals_], &detection_raw_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 2 * config_ptr_->num_proposals_], &detection_raw_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 3 * config_ptr_->num_proposals_], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 4 * config_ptr_->num_proposals_], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 5 * config_ptr_->num_proposals_], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code1 = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code1); + + // Extract the boxes + auto code2 = post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + ASSERT_EQ(cudaSuccess, code2); + EXPECT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, CircleNMSTest) +{ + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + std::vector det_boxes3d; + unsigned int det_idx = 42; + unsigned int det_num = 2; + unsigned int cls_idx = 2; + + constexpr float detection_score = 1.f; + constexpr float detection_x = 50.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + const float detection_raw_x = (detection_x - config_ptr_->min_x_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_x_size_); + const float detection_raw_y = (detection_y - config_ptr_->min_y_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_y_size_); + const float detection_raw_z = detection_z; + + constexpr float detection_w = 3.2f; + constexpr float detection_l = 9.3f; + constexpr float detection_h = 1.7f; + constexpr float detection_log_w = std::log(detection_w); + constexpr float detection_log_l = std::log(detection_l); + constexpr float detection_log_h = std::log(detection_h); + + constexpr float detection_yaw = M_PI_4; + constexpr float detection_yaw_sin = std::sin(detection_yaw); + constexpr float detection_yaw_cos = std::cos(detection_yaw); + + // Set the values in the cuda tensor for 2 detections + for (std::size_t i = 0; i < det_num; ++i) { + cudaMemcpy( + &cls_output_d_[cls_idx * config_ptr_->num_proposals_ + det_idx + i], &detection_score, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx + i], &detection_yaw_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx + i + config_ptr_->num_proposals_], &detection_yaw_cos, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i], &detection_raw_x, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + config_ptr_->num_proposals_], &detection_raw_y, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + 2 * config_ptr_->num_proposals_], &detection_raw_z, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + 3 * config_ptr_->num_proposals_], &detection_log_w, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + 4 * config_ptr_->num_proposals_], &detection_log_l, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + 5 * config_ptr_->num_proposals_], &detection_log_h, + 1 * sizeof(float), cudaMemcpyHostToDevice); + } + + auto code1 = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code1); + + // Extract the boxes + auto code2 = post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + ASSERT_EQ(cudaSuccess, code2); + EXPECT_EQ(1, det_boxes3d.size()); +} + +} // namespace lidar_transfusion + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_postprocess_kernel.hpp b/perception/lidar_transfusion/test/test_postprocess_kernel.hpp new file mode 100644 index 0000000000000..60fa7882ecc11 --- /dev/null +++ b/perception/lidar_transfusion/test/test_postprocess_kernel.hpp @@ -0,0 +1,48 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_POSTPROCESS_KERNEL_HPP_ +#define TEST_POSTPROCESS_KERNEL_HPP_ + +#include +#include + +#include + +#include + +namespace lidar_transfusion +{ + +class PostprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + std::unique_ptr post_ptr_{nullptr}; + std::unique_ptr config_ptr_{}; + cudaStream_t stream_{}; + + unsigned int cls_size_{0}; + unsigned int box_size_{0}; + unsigned int dir_cls_size_{0}; + cuda::unique_ptr cls_output_d_{nullptr}; + cuda::unique_ptr box_output_d_{nullptr}; + cuda::unique_ptr dir_cls_output_d_{nullptr}; +}; + +} // namespace lidar_transfusion + +#endif // TEST_POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/test/test_preprocess_kernel.cpp b/perception/lidar_transfusion/test/test_preprocess_kernel.cpp new file mode 100644 index 0000000000000..c1c2a824f0f53 --- /dev/null +++ b/perception/lidar_transfusion/test/test_preprocess_kernel.cpp @@ -0,0 +1,265 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_preprocess_kernel.hpp" + +#include "lidar_transfusion/cuda_utils.hpp" + +#include + +#include +#include +#include + +#include +#include + +namespace lidar_transfusion +{ + +void PreprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + auto voxels_num = std::vector{1, 3, 5}; + auto point_cloud_range = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; + auto voxel_size = std::vector{0.3, 0.3, 8.0}; + auto score_threshold = 0.2f; + auto circle_nms_dist_threshold = 0.5f; + auto yaw_norm_thresholds = std::vector{0.5, 0.5, 0.5}; + config_ptr_ = std::make_unique( + voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, + score_threshold); + pre_ptr_ = std::make_unique(*config_ptr_, stream_); + + voxel_features_size_ = config_ptr_->max_voxels_ * config_ptr_->max_num_points_per_pillar_ * + config_ptr_->num_point_feature_size_; + voxel_num_size_ = config_ptr_->max_voxels_; + voxel_idxs_size_ = config_ptr_->max_voxels_ * config_ptr_->num_point_values_; + + params_input_d_ = cuda::make_unique(); + voxel_features_d_ = cuda::make_unique(voxel_features_size_); + voxel_num_d_ = cuda::make_unique(voxel_num_size_); + voxel_idxs_d_ = cuda::make_unique(voxel_idxs_size_); + points_d_ = + cuda::make_unique(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); +} + +void PreprocessKernelTest::TearDown() +{ +} + +TEST_F(PreprocessKernelTest, EmptyVoxelTest) +{ + std::vector points{}; + std::size_t count = 0; + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + unsigned int params_input; + auto code1 = cudaStreamSynchronize(stream_); + + auto code2 = cudaMemcpyAsync( + ¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + auto code3 = cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + ASSERT_EQ(cudaSuccess, code1); + ASSERT_EQ(cudaSuccess, code2); + ASSERT_EQ(cudaSuccess, code3); + + EXPECT_EQ(0, params_input); +} + +TEST_F(PreprocessKernelTest, BasicTest) +{ + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_, stream_); + cudaStreamSynchronize(stream_); + + std::vector point1{25.f, -61.1f, 1.8f, 42.0, 0.1f}; + std::vector point2{-31.6f, 1.1f, 2.8f, 77.0, 0.1f}; + std::vector point3{42.6f, 15.1f, -0.1f, 3.0, 0.1f}; + + std::vector points; + points.reserve(point1.size() + point2.size() + point3.size()); + points.insert(points.end(), point1.begin(), point1.end()); + points.insert(points.end(), point2.begin(), point2.end()); + points.insert(points.end(), point3.begin(), point3.end()); + std::size_t count = 3; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + // Compute the total amount of voxels filled + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + + unsigned int params_input; + CHECK_CUDA_ERROR( + cudaMemcpy(¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + ASSERT_EQ(count, params_input); + + // Check if voxels were filled + unsigned int voxel_num; + + for (std::size_t i = 0; i < count; ++i) { + CHECK_CUDA_ERROR( + cudaMemcpy(&voxel_num, voxel_num_d_.get() + i, sizeof(unsigned int), cudaMemcpyDeviceToHost)); + EXPECT_EQ(1, voxel_num); + } + + // Check grid indices + thrust::host_vector voxel_idxs(config_ptr_->num_point_values_ * count, 0); + unsigned int voxel_idx_gt; + unsigned int voxel_idy_gt; + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_idxs.data(), voxel_idxs_d_.get(), + config_ptr_->num_point_values_ * count * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + for (std::size_t i = 0; i < count; ++i) { + voxel_idx_gt = std::floor( + (points[config_ptr_->num_point_feature_size_ * i + 0] - config_ptr_->min_x_range_) / + config_ptr_->voxel_x_size_); + voxel_idy_gt = std::floor( + (points[config_ptr_->num_point_feature_size_ * i + 1] - config_ptr_->min_y_range_) / + config_ptr_->voxel_y_size_); + + EXPECT_EQ( + voxel_idx_gt, + voxel_idxs[config_ptr_->num_point_values_ * i + 3]); // {0, 0, voxel_idy, VOXEL_IDX} + EXPECT_EQ( + voxel_idy_gt, + voxel_idxs[config_ptr_->num_point_values_ * i + 2]); // {0, 0, VOXEL_IDY, voxel_idx} + } + + // Check voxel features + thrust::host_vector voxel_features( + count * config_ptr_->max_num_points_per_pillar_ * config_ptr_->num_point_feature_size_, 0.f); + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_features.data(), voxel_features_d_.get(), + count * config_ptr_->max_num_points_per_pillar_ * config_ptr_->num_point_feature_size_ * + sizeof(float), + cudaMemcpyDeviceToHost)); + + for (std::size_t i = 0; i < count; ++i) { + for (std::size_t j = 0; j < config_ptr_->num_point_feature_size_; ++j) { + EXPECT_EQ( + points[config_ptr_->num_point_feature_size_ * i + j], + voxel_features + [i * config_ptr_->max_num_points_per_pillar_ * config_ptr_->num_point_feature_size_ + j]); + } + } +} + +TEST_F(PreprocessKernelTest, OutOfRangeTest) +{ + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_, stream_); + cudaStreamSynchronize(stream_); + + std::vector points{25.f, config_ptr_->max_y_range_ + 0.1f, 2.1f, 55.f, 0.1f}; + std::size_t count = 0; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + + unsigned int params_input; + CHECK_CUDA_ERROR( + cudaMemcpy(¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + EXPECT_EQ(count, params_input); +} + +TEST_F(PreprocessKernelTest, VoxelOverflowTest) +{ + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_, stream_); + cudaStreamSynchronize(stream_); + + std::vector point{config_ptr_->min_x_range_, -61.1f, 1.8f, 42.f, 0.1f}; + std::size_t count = config_ptr_->max_voxels_ + 1; + std::vector points{}; + points.reserve(count * config_ptr_->num_point_feature_size_); + + for (std::size_t i = 0; i < count; ++i) { + points.insert(points.end(), point.begin(), point.end()); + point[0] += config_ptr_->voxel_x_size_; + } + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + + ASSERT_EQ(cudaSuccess, cudaGetLastError()); + + unsigned int params_input; + CHECK_CUDA_ERROR( + cudaMemcpy(¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + EXPECT_EQ(config_ptr_->max_voxels_, params_input); +} + +} // namespace lidar_transfusion + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_preprocess_kernel.hpp b/perception/lidar_transfusion/test/test_preprocess_kernel.hpp new file mode 100644 index 0000000000000..024fae5eae4b1 --- /dev/null +++ b/perception/lidar_transfusion/test/test_preprocess_kernel.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_PREPROCESS_KERNEL_HPP_ +#define TEST_PREPROCESS_KERNEL_HPP_ + +#include +#include + +#include + +#include + +namespace lidar_transfusion +{ + +class PreprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + std::unique_ptr pre_ptr_{nullptr}; + std::unique_ptr config_ptr_{}; + cudaStream_t stream_{}; + + unsigned int voxel_features_size_{0}; + unsigned int voxel_num_size_{0}; + unsigned int voxel_idxs_size_{0}; + cuda::unique_ptr points_d_{nullptr}; + cuda::unique_ptr params_input_d_{nullptr}; + cuda::unique_ptr voxel_features_d_{nullptr}; + cuda::unique_ptr voxel_num_d_{nullptr}; + cuda::unique_ptr voxel_idxs_d_{nullptr}; +}; + +} // namespace lidar_transfusion + +#endif // TEST_PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/test/test_ros_utils.cpp b/perception/lidar_transfusion/test/test_ros_utils.cpp new file mode 100644 index 0000000000000..15541e6353b42 --- /dev/null +++ b/perception/lidar_transfusion/test/test_ros_utils.cpp @@ -0,0 +1,104 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/ros_utils.hpp" + +#include + +TEST(TestSuite, box3DToDetectedObject) +{ + std::vector class_names = {"CAR", "TRUCK", "BUS", "TRAILER", + "BICYCLE", "MOTORCYCLE", "PEDESTRIAN"}; + + // Test case 1: Test with valid label + { + lidar_transfusion::Box3D box3d; + box3d.label = 0; // CAR + box3d.score = 0.8f; + box3d.x = 1.0; + box3d.y = 2.0; + box3d.z = 3.0; + box3d.width = 2.0; + box3d.height = 1.5; + box3d.length = 4.0; + box3d.yaw = 0.5; + + autoware_perception_msgs::msg::DetectedObject obj; + lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); + + EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f); + EXPECT_EQ( + obj.classification[0].label, autoware_perception_msgs::msg::ObjectClassification::CAR); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.x, 1.0); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.y, 2.0); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.z, 3.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.x, 4.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.y, 2.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.z, 1.5); + EXPECT_FALSE(obj.kinematics.has_position_covariance); + EXPECT_FALSE(obj.kinematics.has_twist); + EXPECT_FALSE(obj.kinematics.has_twist_covariance); + } + + // Test case 2: Test with invalid label + { + lidar_transfusion::Box3D box3d; + box3d.score = 0.5f; + box3d.label = 10; // Invalid + + autoware_perception_msgs::msg::DetectedObject obj; + lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); + + EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f); + EXPECT_EQ( + obj.classification[0].label, autoware_perception_msgs::msg::ObjectClassification::UNKNOWN); + EXPECT_FALSE(obj.kinematics.has_position_covariance); + EXPECT_FALSE(obj.kinematics.has_twist); + EXPECT_FALSE(obj.kinematics.has_twist_covariance); + } +} + +TEST(TestSuite, getSemanticType) +{ + EXPECT_EQ( + lidar_transfusion::getSemanticType("CAR"), + autoware_perception_msgs::msg::ObjectClassification::CAR); + EXPECT_EQ( + lidar_transfusion::getSemanticType("TRUCK"), + autoware_perception_msgs::msg::ObjectClassification::TRUCK); + EXPECT_EQ( + lidar_transfusion::getSemanticType("BUS"), + autoware_perception_msgs::msg::ObjectClassification::BUS); + EXPECT_EQ( + lidar_transfusion::getSemanticType("TRAILER"), + autoware_perception_msgs::msg::ObjectClassification::TRAILER); + EXPECT_EQ( + lidar_transfusion::getSemanticType("MOTORCYCLE"), + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE); + EXPECT_EQ( + lidar_transfusion::getSemanticType("BICYCLE"), + autoware_perception_msgs::msg::ObjectClassification::BICYCLE); + EXPECT_EQ( + lidar_transfusion::getSemanticType("PEDESTRIAN"), + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN); + EXPECT_EQ( + lidar_transfusion::getSemanticType("UNKNOWN"), + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_voxel_generator.cpp b/perception/lidar_transfusion/test/test_voxel_generator.cpp new file mode 100644 index 0000000000000..2673a341b3721 --- /dev/null +++ b/perception/lidar_transfusion/test/test_voxel_generator.cpp @@ -0,0 +1,284 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_voxel_generator.hpp" + +#include "gtest/gtest.h" +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +#include "sensor_msgs/point_cloud2_iterator.hpp" + +namespace lidar_transfusion +{ + +void VoxelGeneratorTest::SetUp() +{ + // Setup things that should occur before every test instance should go here + node_ = std::make_shared("voxel_generator_test_node"); + + world_frame_ = "map"; + lidar_frame_ = "lidar"; + delta_pointcloud_x_ = 1.0; + points_per_pointcloud_ = 300; + + voxels_num_ = std::vector{5000, 30000, 60000}; + point_cloud_range_ = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; + voxel_size_ = std::vector{0.3, 0.3, 8.0}; + score_threshold_ = 0.2f; + circle_nms_dist_threshold_ = 0.5f; + yaw_norm_thresholds_ = std::vector{0.5, 0.5, 0.5}; + config_ptr_ = std::make_unique( + voxels_num_, point_cloud_range_, voxel_size_, circle_nms_dist_threshold_, yaw_norm_thresholds_, + score_threshold_); + + cloud1_ = std::make_unique(); + cloud2_ = std::make_unique(); + + // Set up the fields + point_cloud_msg_wrapper::PointCloud2Modifier< + autoware_point_types::PointXYZIRCAEDT, autoware_point_types::PointXYZIRCAEDTGenerator> + modifier{*cloud1_, lidar_frame_}; + + // Resize the cloud to hold points_per_pointcloud_ points + modifier.resize(points_per_pointcloud_); + + // Create an iterator for desired fields + sensor_msgs::PointCloud2Iterator iter_x(*cloud1_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud1_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud1_, "z"); + sensor_msgs::PointCloud2Iterator iter_i(*cloud1_, "intensity"); + + // Populate the point cloud + for (size_t i = 0; i < modifier.size(); ++i, ++iter_x, ++iter_y, ++iter_z) { + *iter_x = static_cast(i); + *iter_y = static_cast(i); + *iter_z = static_cast(i); + } + for (size_t i = 0; i < modifier.size(); ++i, ++iter_i) { + *iter_i = static_cast(i % 256); + } + + *cloud2_ = *cloud1_; + + // Set the stamps for the point clouds. They usually come every 100ms + cloud1_->header.stamp.sec = 1; + cloud1_->header.stamp.nanosec = 100'000'000; + cloud2_->header.stamp.sec = 1; + cloud2_->header.stamp.nanosec = 200'000'000; + + tf2_buffer_ = std::make_unique(node_->get_clock()); + tf2_buffer_->setUsingDedicatedThread(true); + + // The vehicle moves 1m/s in the x direction + const double world_origin_x = 6'371'000.0; + const double world_origin_y = 1'371'000.0; + + transform1_.header.stamp = cloud1_->header.stamp; + transform1_.header.frame_id = world_frame_; + transform1_.child_frame_id = lidar_frame_; + transform1_.transform.translation.x = world_origin_x; + transform1_.transform.translation.y = world_origin_y; + transform1_.transform.translation.z = 1.8; + transform1_.transform.rotation.x = 0.0; + transform1_.transform.rotation.y = 0.0; + transform1_.transform.rotation.z = 0.0; + transform1_.transform.rotation.w = 1.0; + + transform2_ = transform1_; + transform2_.header.stamp = cloud2_->header.stamp; + transform2_.transform.translation.x = world_origin_x + delta_pointcloud_x_; + + cudaStreamCreate(&stream_); +} + +void VoxelGeneratorTest::TearDown() +{ +} + +TEST_F(VoxelGeneratorTest, CloudInfo) +{ + CloudInfo cloud_info{}; + EXPECT_EQ(cloud1_->is_bigendian, cloud_info.is_bigendian); + EXPECT_EQ(cloud1_->fields[0].name, "x"); + EXPECT_EQ(cloud1_->fields[0].datatype, cloud_info.x_datatype); + EXPECT_EQ(cloud1_->fields[0].count, cloud_info.x_count); + EXPECT_EQ(cloud1_->fields[0].offset, cloud_info.x_offset); + EXPECT_EQ(cloud1_->fields[1].name, "y"); + EXPECT_EQ(cloud1_->fields[1].datatype, cloud_info.y_datatype); + EXPECT_EQ(cloud1_->fields[1].count, cloud_info.y_count); + EXPECT_EQ(cloud1_->fields[1].offset, cloud_info.y_offset); + EXPECT_EQ(cloud1_->fields[2].name, "z"); + EXPECT_EQ(cloud1_->fields[2].datatype, cloud_info.z_datatype); + EXPECT_EQ(cloud1_->fields[2].count, cloud_info.z_count); + EXPECT_EQ(cloud1_->fields[2].offset, cloud_info.z_offset); + EXPECT_EQ(cloud1_->fields[3].name, "intensity"); + EXPECT_EQ(cloud1_->fields[3].datatype, cloud_info.intensity_datatype); + EXPECT_EQ(cloud1_->fields[3].count, cloud_info.intensity_count); + EXPECT_EQ(cloud1_->fields[3].offset, cloud_info.intensity_offset); + EXPECT_EQ(cloud1_->width, points_per_pointcloud_); + EXPECT_EQ(cloud1_->height, 1); +} + +TEST_F(VoxelGeneratorTest, SingleFrame) +{ + const unsigned int num_past_frames = 0; // only current frame + DensificationParam param(world_frame_, num_past_frames); + VoxelGenerator voxel_generator(param, *config_ptr_, stream_); + std::vector points; + points.resize(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + auto points_d = + cuda::make_unique(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), + points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(*cloud1_, points_d); + + cudaMemcpy( + points.data(), points_d.get(), + points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); + + EXPECT_TRUE(status1); + EXPECT_EQ(generated_points_num, points_per_pointcloud_); + + // Check valid points + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 2]); + EXPECT_EQ(static_cast(i % 256), points[i * config_ptr_->num_point_feature_size_ + 3]); + EXPECT_EQ(static_cast(0), points[i * config_ptr_->num_point_feature_size_ + 4]); + } + + // Check invalid points + for (std::size_t i = points_per_pointcloud_ * config_ptr_->num_point_feature_size_; + i < points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * 2; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} + +TEST_F(VoxelGeneratorTest, TwoFramesNoTf) +{ + const unsigned int num_past_frames = 1; + DensificationParam param(world_frame_, num_past_frames); + VoxelGenerator voxel_generator(param, *config_ptr_, stream_); + std::vector points; + points.resize(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + auto points_d = + cuda::make_unique(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), + points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(*cloud1_, points_d); + + cudaMemcpy( + points.data(), points_d.get(), + points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); + + EXPECT_FALSE(status1); + EXPECT_FALSE(status2); + EXPECT_EQ(0, generated_points_num); +} + +TEST_F(VoxelGeneratorTest, TwoFrames) +{ + const unsigned int num_past_frames = 1; + DensificationParam param(world_frame_, num_past_frames); + VoxelGenerator voxel_generator(param, *config_ptr_, stream_); + std::vector points; + points.resize(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + auto points_d = + cuda::make_unique(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), + 2 * points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + tf2_buffer_->setTransform(transform1_, "authority1"); + tf2_buffer_->setTransform(transform2_, "authority1"); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(*cloud1_, points_d); + + cudaMemcpy( + points.data(), points_d.get(), + 2 * points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); + + EXPECT_TRUE(status1); + EXPECT_TRUE(status2); + EXPECT_EQ(2 * points_per_pointcloud_, generated_points_num); + + // Check valid points for the latest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 2]); + EXPECT_EQ(static_cast(i % 256), points[i * config_ptr_->num_point_feature_size_ + 3]); + EXPECT_EQ(static_cast(0), points[i * config_ptr_->num_point_feature_size_ + 4]); + } + + // Check valid points for the oldest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are tf conversions, so results are not numerically the same + EXPECT_NEAR( + static_cast(i) - delta_pointcloud_x_, + points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 0], 1e-6); + EXPECT_NEAR( + static_cast(i), + points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 1], 1e-6); + EXPECT_NEAR( + static_cast(i), + points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 2], 1e-6); + EXPECT_NEAR( + static_cast(i % 256), + points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 3], 1e-6); + EXPECT_NEAR( + 0.1, points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 4], 1e-6); + } + + // Check invalid points + for (std::size_t i = 2 * points_per_pointcloud_ * config_ptr_->num_point_feature_size_; + i < 3 * points_per_pointcloud_ * config_ptr_->num_point_feature_size_; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} +} // namespace lidar_transfusion + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_voxel_generator.hpp b/perception/lidar_transfusion/test/test_voxel_generator.hpp new file mode 100644 index 0000000000000..eccbe6412d183 --- /dev/null +++ b/perception/lidar_transfusion/test/test_voxel_generator.hpp @@ -0,0 +1,65 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_VOXEL_GENERATOR_HPP_ +#define TEST_VOXEL_GENERATOR_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace lidar_transfusion +{ + +class VoxelGeneratorTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + // These need to be public so that they can be accessed in the test cases + rclcpp::Node::SharedPtr node_{}; + + std::unique_ptr cloud1_{}, cloud2_{}; + geometry_msgs::msg::TransformStamped transform1_{}, transform2_{}; + + std::unique_ptr densification_param_ptr_{}; + std::unique_ptr config_ptr_{}; + + std::unique_ptr tf2_buffer_{}; + + std::string world_frame_{}; + std::string lidar_frame_{}; + double delta_pointcloud_x_{}; + std::size_t points_per_pointcloud_{}; + + std::vector voxels_num_{}; + std::vector point_cloud_range_{}; + std::vector voxel_size_{}; + float circle_nms_dist_threshold_{}; + std::vector yaw_norm_thresholds_{}; + float score_threshold_{}; + + cudaStream_t stream_{}; +}; +} // namespace lidar_transfusion + +#endif // TEST_VOXEL_GENERATOR_HPP_ diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 4f9268583dd34..370e5bb0b9161 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -20,32 +20,34 @@ include_directories( ) # Generate exe file -set(MULTI_OBJECT_TRACKER_SRC - src/multi_object_tracker_core.cpp +set(${PROJECT_NAME}_src + src/multi_object_tracker_node.cpp src/debugger/debugger.cpp src/debugger/debug_object.cpp src/processor/processor.cpp src/processor/input_manager.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp - src/tracker/motion_model/motion_model_base.cpp - src/tracker/motion_model/bicycle_motion_model.cpp +) +set(${PROJECT_NAME}_lib + lib/association/association.cpp + lib/association/mu_successive_shortest_path/mu_ssp.cpp + lib/tracker/motion_model/motion_model_base.cpp + lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv - src/tracker/motion_model/ctrv_motion_model.cpp - src/tracker/motion_model/cv_motion_model.cpp - src/tracker/model/tracker_base.cpp - src/tracker/model/big_vehicle_tracker.cpp - src/tracker/model/normal_vehicle_tracker.cpp - src/tracker/model/multiple_vehicle_tracker.cpp - src/tracker/model/bicycle_tracker.cpp - src/tracker/model/pedestrian_tracker.cpp - src/tracker/model/pedestrian_and_bicycle_tracker.cpp - src/tracker/model/unknown_tracker.cpp - src/tracker/model/pass_through_tracker.cpp + lib/tracker/motion_model/ctrv_motion_model.cpp + lib/tracker/motion_model/cv_motion_model.cpp + lib/tracker/model/tracker_base.cpp + lib/tracker/model/big_vehicle_tracker.cpp + lib/tracker/model/normal_vehicle_tracker.cpp + lib/tracker/model/multiple_vehicle_tracker.cpp + lib/tracker/model/bicycle_tracker.cpp + lib/tracker/model/pedestrian_tracker.cpp + lib/tracker/model/pedestrian_and_bicycle_tracker.cpp + lib/tracker/model/unknown_tracker.cpp + lib/tracker/model/pass_through_tracker.cpp ) - ament_auto_add_library(${PROJECT_NAME} SHARED - ${MULTI_OBJECT_TRACKER_SRC} + ${${PROJECT_NAME}_src} + ${${PROJECT_NAME}_lib} ) target_link_libraries(${PROJECT_NAME} @@ -54,8 +56,8 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "multi_object_tracker::MultiObjectTracker" - EXECUTABLE ${PROJECT_NAME}_node + PLUGIN "autoware::multi_object_tracker::MultiObjectTracker" + EXECUTABLE multi_object_tracker_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp similarity index 81% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index c30423db02c26..2c12341d0aa67 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -16,13 +16,13 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/data_association/solver/gnn_solver.hpp" -#include "multi_object_tracker/tracker/tracker.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/tracker/tracker.hpp" #include #include @@ -34,6 +34,8 @@ #include #include +namespace autoware::multi_object_tracker +{ class DataAssociation { private: @@ -61,4 +63,6 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__ASSOCIATION_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp similarity index 55% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp index 631fd73f8583d..3b72b800e86bc 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" -#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" -#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/mu_ssp.hpp" +#include "autoware/multi_object_tracker/association/solver/ssp.hpp" -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 73% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp index 1fb508bcf12b2..88f9181d96d9b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/gnn_solver_interface.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class GnnSolverInterface @@ -30,6 +32,8 @@ class GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) = 0; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp index a4d9d727e6cc5..024670c3389c6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class MuSSP : public GnnSolverInterface @@ -32,6 +34,8 @@ class MuSSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp index 1c4b15cd68b59..0f987fc49ee7c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/solver/successive_shortest_path.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp" #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { class SSP : public GnnSolverInterface @@ -32,6 +34,8 @@ class SSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; + } // namespace gnn_solver +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 63ad496b70ed9..5374e28d5f9cf 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class BicycleTracker : public Tracker { @@ -67,4 +70,6 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) const; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 7112e6a08ade1..c3f824aff35b4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class BigVehicleTracker : public Tracker { @@ -70,4 +73,6 @@ class BigVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 75aec0b06d6b8..a0a6bd7781761 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -16,16 +16,19 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include +namespace autoware::multi_object_tracker +{ + class MultipleVehicleTracker : public Tracker { private: @@ -48,4 +51,6 @@ class MultipleVehicleTracker : public Tracker virtual ~MultipleVehicleTracker() {} }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 1165cecab258b..f7edebfc31378 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" + +namespace autoware::multi_object_tracker +{ class NormalVehicleTracker : public Tracker { @@ -70,4 +73,6 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp similarity index 81% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index cd661dab52c6e..e7f0a5fd699e1 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -16,12 +16,15 @@ // Author: v1.0 Yutaka Shimizu // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #include "kalman_filter/kalman_filter.hpp" #include "tracker_base.hpp" +namespace autoware::multi_object_tracker +{ + class PassThroughTracker : public Tracker { private: @@ -44,4 +47,6 @@ class PassThroughTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp similarity index 71% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 3c3ac038b085e..f593280c2e183 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -16,13 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" -#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" + +namespace autoware::multi_object_tracker +{ class PedestrianAndBicycleTracker : public Tracker { @@ -46,4 +49,6 @@ class PedestrianAndBicycleTracker : public Tracker virtual ~PedestrianAndBicycleTracker() {} }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp similarity index 78% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c5be57f656eb5..1e0f778a69137 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -16,15 +16,16 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "multi_object_tracker/tracker/object_model/object_model.hpp" -// cspell: ignore CTRV +namespace autoware::multi_object_tracker +{ class PedestrianTracker : public Tracker { @@ -49,7 +50,7 @@ class PedestrianTracker : public Tracker }; BoundingBox bounding_box_; Cylinder cylinder_; - + // cspell: ignore CTRV CTRVMotionModel motion_model_; using IDX = CTRVMotionModel::IDX; @@ -75,4 +76,6 @@ class PedestrianTracker : public Tracker const geometry_msgs::msg::Transform & self_transform) const; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 44b3884c392e6..bc52e49f763fd 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -33,6 +33,9 @@ #include +namespace autoware::multi_object_tracker +{ + class Tracker { private: @@ -114,4 +117,6 @@ class Tracker virtual bool predict(const rclcpp::Time & time) = 0; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp similarity index 80% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index ca8ecba160bd8..4bc03f439ffc2 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -16,12 +16,15 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" + +namespace autoware::multi_object_tracker +{ class UnknownTracker : public Tracker { @@ -62,4 +65,6 @@ class UnknownTracker : public Tracker autoware_perception_msgs::msg::TrackedObject & object) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp similarity index 87% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index f6ce2842388c6..6e20d31aad168 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,6 +32,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class BicycleMotionModel : public MotionModel { private: @@ -107,4 +110,6 @@ class BicycleMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp similarity index 86% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 657048079c46c..3cca786d9f65b 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,8 +32,10 @@ #endif #include -// cspell: ignore CTRV +namespace autoware::multi_object_tracker +{ +// cspell: ignore CTRV class CTRVMotionModel : public MotionModel { private: @@ -92,4 +94,6 @@ class CTRVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp similarity index 84% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 421e413d7aab7..ad3061285a80c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -16,11 +16,11 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "kalman_filter/kalman_filter.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include #include @@ -32,6 +32,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class CVMotionModel : public MotionModel { private: @@ -85,4 +88,6 @@ class CVMotionModel : public MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp similarity index 85% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp index e740612e96d4f..130053fafd2ed 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #include "kalman_filter/kalman_filter.hpp" @@ -31,6 +31,9 @@ #endif #include +namespace autoware::multi_object_tracker +{ + class MotionModel { private: @@ -65,4 +68,6 @@ class MotionModel geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; }; -#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp similarity index 96% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp index 6a771344bb36b..b49464109eec8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ #include @@ -46,6 +46,9 @@ constexpr T kmph2mps(const T kmph) } // namespace +namespace autoware::multi_object_tracker +{ + namespace object_model { @@ -301,5 +304,6 @@ static const ObjectModel bicycle(ObjectModelType::Bicycle); static const ObjectModel pedestrian(ObjectModelType::Pedestrian); } // namespace object_model +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp similarity index 84% rename from perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp index a8bc58e254fc2..ea1e60d4c7e17 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ -#define MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #include "model/bicycle_tracker.hpp" #include "model/big_vehicle_tracker.hpp" @@ -29,4 +29,4 @@ #include "model/tracker_base.hpp" #include "model/unknown_tracker.hpp" -#endif // MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp similarity index 98% rename from perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp rename to perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp index 0bda7870ae2b9..833f768e171f4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ #include #include @@ -276,4 +276,4 @@ inline bool getMeasurementYaw( } // namespace utils -#endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/lib/association/association.cpp similarity index 96% rename from perception/multi_object_tracker/src/data_association/data_association.cpp rename to perception/multi_object_tracker/lib/association/association.cpp index f7b40cd178528..ac31cad4cc13c 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/lib/association/association.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/data_association.hpp" +#include "autoware/multi_object_tracker/association/association.hpp" -#include "multi_object_tracker/data_association/solver/gnn_solver.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -68,6 +68,9 @@ double getFormedYawAngle( } } // namespace +namespace autoware::multi_object_tracker +{ + DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_area_vector, std::vector min_area_vector, @@ -224,3 +227,5 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( return score_matrix; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp similarity index 88% rename from perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp rename to perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp index 18779a7fffbdf..9ae6e300b2b9b 100644 --- a/perception/multi_object_tracker/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/mu_ssp.hpp" #include @@ -24,6 +24,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { void MuSSP::maximizeLinearAssignment( @@ -39,3 +41,5 @@ void MuSSP::maximizeLinearAssignment( solve_muSSP(cost, direct_assignment, reverse_assignment); } } // namespace gnn_solver + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp similarity index 96% rename from perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp rename to perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp index 0b52c09cb54b8..84333fe8a1ff6 100644 --- a/perception/multi_object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp" +#include "autoware/multi_object_tracker/association/solver/ssp.hpp" #include #include @@ -22,6 +22,8 @@ #include #include +namespace autoware::multi_object_tracker +{ namespace gnn_solver { struct ResidualEdge @@ -328,12 +330,12 @@ void SSP::maximizeLinearAssignment( #ifndef NDEBUG // Check if the potentials are feasible potentials - for (int v = 0; v < n_nodes; ++v) { - for (auto it_incident_edge = adjacency_list.at(v).cbegin(); - it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + for (int j = 0; j < n_nodes; ++j) { + for (auto it_incident_edge = adjacency_list.at(j).cbegin(); + it_incident_edge != adjacency_list.at(j).cend(); ++it_incident_edge) { if (it_incident_edge->capacity > 0) { double reduced_cost = - it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + it_incident_edge->cost + potentials.at(j) - potentials.at(it_incident_edge->dst); assert(reduced_cost >= 0); } } @@ -368,3 +370,5 @@ void SSP::maximizeLinearAssignment( #endif } } // namespace gnn_solver + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index a438c830596d7..ba53ccec1ad43 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( @@ -334,3 +337,5 @@ bool BicycleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp index 90d33e65b46bd..0593b7fc9dc12 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,8 @@ #include #endif +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( @@ -398,3 +400,5 @@ bool BigVehicleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 67445e5b0daa2..a6e5021e4fe7d 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Yukihiro Saito // -#include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" + +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -65,3 +68,5 @@ bool MultipleVehicleTracker::getTrackedObject( object.classification = getClassification(); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp index 67282893083c1..ef345692b32ca 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( @@ -399,3 +402,5 @@ bool NormalVehicleTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp similarity index 95% rename from perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 17be415480360..c872944fab3d7 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -16,10 +16,10 @@ // Author: v1.0 Yutaka Shimizu // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -36,6 +36,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -117,3 +120,5 @@ bool PassThroughTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 00609934990a3..8c665e6078a2b 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Yukihiro Saito // -#include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" + +namespace autoware::multi_object_tracker +{ using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -65,3 +68,5 @@ bool PedestrianAndBicycleTracker::getTrackedObject( object.classification = getClassification(); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index d9412c3b5739c..ee50c2e5449ed 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -17,13 +17,13 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include @@ -40,6 +40,9 @@ #include #endif +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( @@ -338,3 +341,5 @@ bool PedestrianTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp similarity index 96% rename from perception/multi_object_tracker/src/tracker/model/tracker_base.cpp rename to perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp index f27bad172a4b9..daa8db5db91e6 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -14,9 +14,9 @@ // // -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include @@ -40,6 +40,9 @@ float decayProbability(const float & prior, const float & delta_time) } } // namespace +namespace autoware::multi_object_tracker +{ + Tracker::Tracker( const rclcpp::Time & time, const std::vector & classification, @@ -202,3 +205,5 @@ geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp rename to perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 6905d7c3b8ced..0648007e0b807 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -13,13 +13,13 @@ // limitations under the License. #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include @@ -36,6 +36,9 @@ #endif #include "object_recognition_utils/object_recognition_utils.hpp" +namespace autoware::multi_object_tracker +{ + UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -234,3 +237,5 @@ bool UnknownTracker::getTrackedObject( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp similarity index 98% rename from perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 466108a2bef0e..2e325d18579a6 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -17,17 +17,20 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include +namespace autoware::multi_object_tracker +{ + // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity @@ -487,3 +490,5 @@ bool BicycleMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index b19af1d26d162..b1c5a36f9ad5b 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -17,17 +17,19 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include +namespace autoware::multi_object_tracker +{ // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; @@ -386,3 +388,5 @@ bool CTRVMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp similarity index 97% rename from perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index 231ba73796ed9..2e2ba660a6e0d 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -17,19 +17,21 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/math/normalization.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include #include #include +namespace autoware::multi_object_tracker +{ // cspell: ignore CV // Constant Velocity (CV) motion model using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; @@ -306,3 +308,5 @@ bool CVMotionModel::getPredictedState( return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp similarity index 93% rename from perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp rename to perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp index 4d51021de634b..ffbb452815fa7 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp +++ b/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp @@ -16,7 +16,10 @@ // Author: v1.0 Taekjin Lee // -#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +namespace autoware::multi_object_tracker +{ MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) { @@ -90,3 +93,5 @@ bool MotionModel::getPredictedState( tmp_ekf_for_no_update.getP(P); return true; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index fe6d3d5c75564..58b7dfc5cef48 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/debugger/debug_object.hpp" +#include "debug_object.hpp" #include "autoware_perception_msgs/msg/tracked_object.hpp" @@ -48,6 +48,9 @@ int32_t uuidToInt(const boost::uuids::uuid & uuid) } } // namespace +namespace autoware::multi_object_tracker +{ + TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) { // set frame id @@ -79,7 +82,7 @@ void TrackerObjectDebugger::collect( const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { - if (!is_initialized_) is_initialized_ = true; + is_initialized_ = true; message_time_ = message_time; @@ -391,3 +394,5 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma return; } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/src/debugger/debug_object.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp rename to perception/multi_object_tracker/src/debugger/debug_object.hpp index 08c9eb975d8e6..ed4c556959861 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +#ifndef DEBUGGER__DEBUG_OBJECT_HPP_ +#define DEBUGGER__DEBUG_OBJECT_HPP_ +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/universe_utils/ros/uuid_helper.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -37,6 +37,9 @@ #include #include +namespace autoware::multi_object_tracker +{ + struct ObjectData { rclcpp::Time time; @@ -99,4 +102,6 @@ class TrackerObjectDebugger void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index e692ae76468e4..3e47bbe9bed8d 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/debugger/debugger.hpp" +#include "debugger.hpp" #include +namespace autoware::multi_object_tracker +{ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id) : node_(node), diagnostic_updater_(&node), object_debugger_(frame_id) { @@ -207,3 +209,5 @@ void TrackerDebugger::publishObjectsMarkers() // reset object data object_debugger_.reset(); } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/src/debugger/debugger.hpp similarity index 94% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp rename to perception/multi_object_tracker/src/debugger/debugger.hpp index a1c516147b220..77618e1882be7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/src/debugger/debugger.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#ifndef DEBUGGER__DEBUGGER_HPP_ +#define DEBUGGER__DEBUGGER_HPP_ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "multi_object_tracker/debugger/debug_object.hpp" +#include "debug_object.hpp" #include #include @@ -33,6 +33,9 @@ #include #include +namespace autoware::multi_object_tracker +{ + /** * @brief Debugger class for multi object tracker * @details This class is used to publish debug information of multi object tracker @@ -104,4 +107,6 @@ class TrackerDebugger void publishObjectsMarkers(); }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // DEBUGGER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp similarity index 94% rename from perception/multi_object_tracker/src/multi_object_tracker_core.cpp rename to perception/multi_object_tracker/src/multi_object_tracker_node.cpp index 0a1f5f2344ba7..45955ac983dde 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -15,13 +15,12 @@ // #define EIGEN_MPL2_ONLY -#include "multi_object_tracker/multi_object_tracker_core.hpp" +#include "multi_object_tracker_node.hpp" -#include "multi_object_tracker/utils/utils.hpp" +#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include -#include #include @@ -66,7 +65,7 @@ boost::optional getTransformAnonymous( } // namespace -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -192,7 +191,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); - data_association_ = std::make_unique( + association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); } @@ -296,9 +295,9 @@ void MultiObjectTracker::runProcess( const auto & list_tracker = processor_->getListTracker(); const auto & detected_objects = transformed_objects; // global nearest neighbor - Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( + Eigen::MatrixXd score_matrix = association_->calcScoreMatrix( detected_objects, list_tracker); // row : tracker, col : measurement - data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + association_->assign(score_matrix, direct_assignment, reverse_assignment); // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( @@ -351,14 +350,16 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - TrackedObjects tentative_objects_msg; - tentative_objects_msg.header.frame_id = world_frame_id_; - processor_->getTentativeObjects(time, tentative_objects_msg); - debugger_->publishTentativeObjects(tentative_objects_msg); + TrackedObjects tentative_output_msg; + tentative_output_msg.header.frame_id = world_frame_id_; + processor_->getTentativeObjects(time, tentative_output_msg); + debugger_->publishTentativeObjects(tentative_output_msg); } debugger_->publishObjectsMarkers(); } -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker + +#include -RCLCPP_COMPONENTS_REGISTER_NODE(multi_object_tracker::MultiObjectTracker) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::multi_object_tracker::MultiObjectTracker) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/src/multi_object_tracker_node.hpp similarity index 82% rename from perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp rename to perception/multi_object_tracker/src/multi_object_tracker_node.hpp index aff3cbd00eabe..db5eaaa88ca8c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.hpp @@ -16,14 +16,14 @@ // Author: v1.0 Yukihiro Saito /// -#ifndef MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ -#define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ +#ifndef MULTI_OBJECT_TRACKER_NODE_HPP_ +#define MULTI_OBJECT_TRACKER_NODE_HPP_ -#include "multi_object_tracker/data_association/data_association.hpp" -#include "multi_object_tracker/debugger/debugger.hpp" -#include "multi_object_tracker/processor/input_manager.hpp" -#include "multi_object_tracker/processor/processor.hpp" -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/association/association.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "debugger/debugger.hpp" +#include "processor/input_manager.hpp" +#include "processor/processor.hpp" #include @@ -52,7 +52,7 @@ #include #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using DetectedObject = autoware_perception_msgs::msg::DetectedObject; @@ -82,7 +82,7 @@ class MultiObjectTracker : public rclcpp::Node // internal states std::string world_frame_id_; // tracking frame - std::unique_ptr data_association_; + std::unique_ptr association_; std::unique_ptr processor_; // input manager @@ -103,6 +103,6 @@ class MultiObjectTracker : public rclcpp::Node inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ +#endif // MULTI_OBJECT_TRACKER_NODE_HPP_ diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index bee76e6c05940..5172cc5062450 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/processor/input_manager.hpp" +#include "input_manager.hpp" #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { /////////////////////////// /////// InputStream /////// @@ -62,7 +62,7 @@ void InputStream::onMessage( { const DetectedObjects objects = *msg; objects_que_.push_back(objects); - if (objects_que_.size() > que_size_) { + while (objects_que_.size() > que_size_) { objects_que_.pop_front(); } @@ -160,21 +160,27 @@ void InputStream::getObjectsOlderThan( return; } - for (const auto & object : objects_que_) { - const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); - - // remove objects older than the specified duration + for (const auto & objects : objects_que_) { + const rclcpp::Time object_time = rclcpp::Time(objects.header.stamp); + // ignore objects older than the specified duration if (object_time < object_oldest_time) { - objects_que_.pop_front(); continue; } // Add the object if the object is older than the specified latest time - if (object_latest_time >= object_time) { - std::pair object_pair(index_, object); - objects_list.push_back(object_pair); - // remove the object from the queue + if (object_time <= object_latest_time) { + std::pair objects_pair(index_, objects); + objects_list.push_back(objects_pair); + } + } + + // remove objects older than 'object_latest_time' + while (!objects_que_.empty()) { + const rclcpp::Time object_time = rclcpp::Time(objects_que_.front().header.stamp); + if (object_time < object_latest_time) { objects_que_.pop_front(); + } else { + break; } } } @@ -368,4 +374,4 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li return is_any_object; } -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/src/processor/input_manager.hpp similarity index 95% rename from perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp rename to perception/multi_object_tracker/src/processor/input_manager.hpp index 29b0c18ae766f..432a4105d815f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/src/processor/input_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#ifndef PROCESSOR__INPUT_MANAGER_HPP_ +#define PROCESSOR__INPUT_MANAGER_HPP_ #include "rclcpp/rclcpp.hpp" @@ -26,7 +26,7 @@ #include #include -namespace multi_object_tracker +namespace autoware::multi_object_tracker { using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; using ObjectsList = std::vector>; @@ -155,6 +155,6 @@ class InputManager void optimizeTimings(); }; -} // namespace multi_object_tracker +} // namespace autoware::multi_object_tracker -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#endif // PROCESSOR__INPUT_MANAGER_HPP_ diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index 50d1a021c5838..17871c89b5258 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "multi_object_tracker/processor/processor.hpp" +#include "processor.hpp" -#include "multi_object_tracker/tracker/tracker.hpp" +#include "autoware/multi_object_tracker/tracker/tracker.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "autoware_perception_msgs/msg/tracked_objects.hpp" #include +namespace autoware::multi_object_tracker +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; TrackerProcessor::TrackerProcessor( @@ -248,3 +251,5 @@ void TrackerProcessor::getTentativeObjects( } } } + +} // namespace autoware::multi_object_tracker diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/src/processor/processor.hpp similarity index 91% rename from perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp rename to perception/multi_object_tracker/src/processor/processor.hpp index 6d0dbcd036e53..5eac113b3974c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/src/processor/processor.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ -#define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +#ifndef PROCESSOR__PROCESSOR_HPP_ +#define PROCESSOR__PROCESSOR_HPP_ -#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -29,6 +29,8 @@ #include #include +namespace autoware::multi_object_tracker +{ class TrackerProcessor { public: @@ -78,4 +80,6 @@ class TrackerProcessor const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; -#endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ +} // namespace autoware::multi_object_tracker + +#endif // PROCESSOR__PROCESSOR_HPP_ diff --git a/perception/object_merger/src/association/solver/successive_shortest_path.cpp b/perception/object_merger/src/association/solver/successive_shortest_path.cpp index 782aa1ef4ac31..e384f12d60051 100644 --- a/perception/object_merger/src/association/solver/successive_shortest_path.cpp +++ b/perception/object_merger/src/association/solver/successive_shortest_path.cpp @@ -328,12 +328,12 @@ void SSP::maximizeLinearAssignment( #ifndef NDEBUG // Check if the potentials are feasible potentials - for (int v = 0; v < n_nodes; ++v) { - for (auto it_incident_edge = adjacency_list.at(v).cbegin(); - it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + for (int w = 0; w < n_nodes; ++w) { + for (auto it_incident_edge = adjacency_list.at(w).cbegin(); + it_incident_edge != adjacency_list.at(w).cend(); ++it_incident_edge) { if (it_incident_edge->capacity > 0) { double reduced_cost = - it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + it_incident_edge->cost + potentials.at(w) - potentials.at(it_incident_edge->dst); assert(reduced_cost >= 0); } } diff --git a/perception/object_merger/src/object_association_merger_node.hpp b/perception/object_merger/src/object_association_merger_node.hpp index d5b5a1ae018a6..d11f58bc5a857 100644 --- a/perception/object_merger/src/object_association_merger_node.hpp +++ b/perception/object_merger/src/object_association_merger_node.hpp @@ -55,8 +55,8 @@ class ObjectAssociationMergerNode : public rclcpp::Node private: void objectsCallback( - const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object0_msg, - const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object1_msg); + const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects0_msg, + const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects1_msg); tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt b/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt index 724e5a3354396..9754cea458577 100644 --- a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt +++ b/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt @@ -8,7 +8,6 @@ find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) find_package(OpenMP) ament_auto_find_build_dependencies() @@ -22,26 +21,26 @@ include_directories( ${GRID_MAP_INCLUDE_DIR} ) -ament_auto_add_library(occupancy_grid_map_outlier_filter SHARED - src/occupancy_grid_map_outlier_filter_nodelet.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/occupancy_grid_map_outlier_filter_node.cpp ) -target_link_libraries(occupancy_grid_map_outlier_filter +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) if(OPENMP_FOUND) - set_target_properties(occupancy_grid_map_outlier_filter PROPERTIES + set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${OpenMP_CXX_FLAGS} LINK_FLAGS ${OpenMP_CXX_FLAGS} ) endif() # -- Occupancy Grid Map Outlier Filter -- -rclcpp_components_register_node(occupancy_grid_map_outlier_filter - PLUGIN "occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent" EXECUTABLE occupancy_grid_map_outlier_filter_node) ament_auto_package(INSTALL_TO_SHARE) diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml index 23d08db09553a..e92e5edb4b084 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -19,27 +19,18 @@ autoware_lanelet2_extension autoware_universe_utils - autoware_vehicle_msgs - diagnostic_updater - image_transport - libopencv-dev libpcl-all-dev message_filters nav_msgs pcl_conversions - pcl_msgs pcl_ros pointcloud_preprocessor rclcpp rclcpp_components sensor_msgs std_msgs - tf2 tf2_eigen - tf2_geometry_msgs tf2_ros - tier4_debug_msgs - tier4_pcl_extensions ament_lint_auto autoware_lint_common diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp similarity index 97% rename from perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp rename to perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp index f62a3f7e689fc..1536db8610b76 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" +#include "occupancy_grid_map_outlier_filter_node.hpp" + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" -#include -#include -#include #include #include @@ -101,7 +102,7 @@ boost::optional getCost( } // namespace -namespace occupancy_grid_map_outlier_filter +namespace autoware::occupancy_grid_map_outlier_filter { RadiusSearch2dFilter::RadiusSearch2dFilter(rclcpp::Node & node) { @@ -489,8 +490,8 @@ void OccupancyGridMapOutlierFilterComponent::Debugger::transformToBaseLink( transformPointcloud(ros_input, *(node_.tf2_), node_.base_link_frame_, output); } -} // namespace occupancy_grid_map_outlier_filter +} // namespace autoware::occupancy_grid_map_outlier_filter #include RCLCPP_COMPONENTS_REGISTER_NODE( - occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent) + autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent) diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp similarity index 91% rename from perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp rename to perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp index ae6d7a34fca2a..e9402d6416cbd 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ -#define OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ +#ifndef OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ +#define OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ +#include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include #include #include @@ -40,7 +40,7 @@ #include #include -namespace occupancy_grid_map_outlier_filter +namespace autoware::occupancy_grid_map_outlier_filter { using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; @@ -129,6 +129,6 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node std::string base_link_frame_; int cost_threshold_; }; -} // namespace occupancy_grid_map_outlier_filter +} // namespace autoware::occupancy_grid_map_outlier_filter -#endif // OCCUPANCY_GRID_MAP_OUTLIER_FILTER__OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODELET_HPP_ +#endif // OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 928532f928e38..d01ef157e21b4 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -14,8 +14,8 @@ include_directories( ) ament_auto_add_library(${PROJECT_NAME}_common SHARED - src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/updater/binary_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(${PROJECT_NAME}_common ${PCL_LIBRARIES} @@ -24,9 +24,9 @@ target_link_libraries(${PROJECT_NAME}_common # PointcloudBasedOccupancyGridMap ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp + lib/costmap_2d/occupancy_grid_map_base.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/costmap_2d/occupancy_grid_map_projective.cpp ) target_link_libraries(pointcloud_based_occupancy_grid_map @@ -35,14 +35,14 @@ target_link_libraries(pointcloud_based_occupancy_grid_map ) rclcpp_components_register_node(pointcloud_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" EXECUTABLE pointcloud_based_occupancy_grid_map_node ) # LaserscanBasedOccupancyGridMap ament_auto_add_library(laserscan_based_occupancy_grid_map SHARED src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp - src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp + lib/costmap_2d/occupancy_grid_map.cpp ) target_link_libraries(laserscan_based_occupancy_grid_map @@ -51,17 +51,17 @@ target_link_libraries(laserscan_based_occupancy_grid_map ) rclcpp_components_register_node(laserscan_based_occupancy_grid_map - PLUGIN "occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" + PLUGIN "autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode" EXECUTABLE laserscan_based_occupancy_grid_map_node ) # GridMapFusionNode ament_auto_add_library(synchronized_grid_map_fusion SHARED src/fusion/synchronized_grid_map_fusion_node.cpp - src/fusion/single_frame_fusion_policy.cpp - src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp - src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp - src/utils/utils.cpp + lib/fusion_policy/fusion_policy.cpp + lib/costmap_2d/occupancy_grid_map_fixed.cpp + lib/updater/log_odds_bayes_filter_updater.cpp + lib/utils/utils.cpp ) target_link_libraries(synchronized_grid_map_fusion @@ -69,7 +69,7 @@ target_link_libraries(synchronized_grid_map_fusion ) rclcpp_components_register_node(synchronized_grid_map_fusion - PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + PLUGIN "autoware::occupancy_grid_map::GridMapFusionNode" EXECUTABLE synchronized_grid_map_fusion_node ) @@ -88,13 +88,14 @@ if(BUILD_TESTING) # gtest ament_add_gtest(test_utils - test/test_utils.cpp + test/test_utils.cpp ) ament_add_gtest(costmap_unit_tests - test/cost_value_test.cpp) + test/cost_value_test.cpp + ) ament_add_gtest(fusion_policy_unit_tests - test/fusion_policy_test.cpp - src/fusion/single_frame_fusion_policy.cpp + test/fusion_policy_test.cpp + lib/fusion_policy/fusion_policy.cpp ) target_link_libraries(test_utils ${PCL_LIBRARIES} diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp similarity index 90% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp index 880297a1210ed..d470d206c41c6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp @@ -48,12 +48,14 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ #include -namespace occupancy_cost_value +namespace autoware::occupancy_grid_map +{ +namespace cost_value { static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; @@ -101,6 +103,8 @@ struct InverseCostTranslationTable static const CostTranslationTable cost_translation_table; static const InverseCostTranslationTable inverse_cost_translation_table; -} // namespace occupancy_cost_value -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +} // namespace cost_value +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE__COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp similarity index 91% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp index d2210cf9c348a..93e768c0f6b4e 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -59,6 +59,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -90,5 +92,6 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp similarity index 66% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index f01b2d74e160b..09ad0a4d554a1 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp @@ -49,16 +49,20 @@ * David V. Lu!! *********************************************************************/ -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ +#include #include #include +#include #include #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -83,13 +87,55 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D virtual void initRosParam(rclcpp::Node & node) = 0; + void setHeightLimit(const double min_height, const double max_height); + + double min_height_; + double max_height_; + + void setFieldOffsets(const PointCloud2 & input_raw, const PointCloud2 & input_obstacle); + + int x_offset_raw_; + int y_offset_raw_; + int z_offset_raw_; + int x_offset_obstacle_; + int y_offset_obstacle_; + int z_offset_obstacle_; + bool offset_initialized_; + + const double min_angle_ = autoware::universe_utils::deg2rad(-180.0); + const double max_angle_ = autoware::universe_utils::deg2rad(180.0); + const double angle_increment_inv_ = 1.0 / autoware::universe_utils::deg2rad(0.1); + + Eigen::Matrix4f mat_map_, mat_scan_; + + bool isPointValid(const Eigen::Vector4f & pt) const + { + // Apply height filter and exclude invalid points + return min_height_ < pt[2] && pt[2] < max_height_ && std::isfinite(pt[0]) && + std::isfinite(pt[1]) && std::isfinite(pt[2]); + } + // Transform pt to (pt_map, pt_scan), then calculate angle_bin_index and range + void transformPointAndCalculate( + const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index, + double & range) const + { + pt_map = mat_map_ * pt; + Eigen::Vector4f pt_scan(mat_scan_ * pt_map); + const double angle = atan2(pt_scan[1], pt_scan[0]); + angle_bin_index = (angle - min_angle_) * angle_increment_inv_; + range = std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]); + } + private: bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; rclcpp::Logger logger_{rclcpp::get_logger("pointcloud_based_occupancy_grid_map")}; rclcpp::Clock clock_{RCL_ROS_TIME}; + + double resolution_inv_; }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp similarity index 72% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp index 298f327d632d7..3301fd1987bd3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -34,6 +36,7 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface using OccupancyGridMapInterface::raytrace; using OccupancyGridMapInterface::setCellValue; + using OccupancyGridMapInterface::setFieldOffsets; using OccupancyGridMapInterface::updateOrigin; void initRosParam(rclcpp::Node & node) override; @@ -43,5 +46,6 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp similarity index 75% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp index 67b51d6184c8c..7569a60b36466 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp @@ -12,15 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using geometry_msgs::msg::Pose; @@ -38,6 +40,7 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface using OccupancyGridMapInterface::raytrace; using OccupancyGridMapInterface::setCellValue; + using OccupancyGridMapInterface::setFieldOffsets; using OccupancyGridMapInterface::updateOrigin; void initRosParam(rclcpp::Node & node) override; @@ -51,5 +54,6 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp similarity index 84% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp index 1f0553878ef5a..a2d7c90a023a5 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -25,6 +25,8 @@ /*min and max prob threshold to prevent log-odds to diverge*/ #define EPSILON_PROB 0.03 +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; @@ -61,6 +63,8 @@ unsigned char singleFrameOccupancyFusion( unsigned char singleFrameOccupancyFusion( const std::vector & occupancy, FusionMethod method, const std::vector & reliability); + } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION_POLICY__FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp similarity index 72% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp index 3a921035ef219..af28b7962b3bc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface @@ -38,5 +40,6 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp similarity index 69% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp index 245484e442609..d7bb1184c06d6 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -25,6 +25,8 @@ // LOBF means: Log Odds Bayes Filter // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface @@ -45,5 +47,6 @@ class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp similarity index 68% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp index e85edf2245ef3..75231089ac33c 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D @@ -27,8 +29,7 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D public: OccupancyGridMapUpdaterInterface( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) - : Costmap2D( - cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) + : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } virtual ~OccupancyGridMapUpdaterInterface() = default; @@ -37,5 +38,6 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D }; } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp rename to perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index 0950272dac470..a8288d2720f48 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include @@ -45,6 +45,8 @@ #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -95,6 +97,8 @@ void transformPointcloud( const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose, sensor_msgs::msg::PointCloud2 & output); +Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose); + bool cropPointcloudByHeight( const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame, const float min_height, const float max_height, @@ -117,6 +121,8 @@ bool extractCommonPointCloud( sensor_msgs::msg::PointCloud2 & output_obstacle_pc); unsigned char getApproximateOccupancyState(const unsigned char & value); + } // namespace utils +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 62cfa4bcb5228..1edc538b3de3f 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -85,7 +85,7 @@ def launch_setup(context, *args, **kwargs): ), ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ("~/input/laserscan", LaunchConfiguration("output/laserscan")), diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index b112dd0a84b83..ef40839d2a5aa 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -196,7 +196,7 @@ def launch_setup(context, *args, **kwargs): # generate composable node node = ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", namespace=frame_name, remappings=[ diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 0e84e4860fdf3..bb5ef025a7e47 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs): composable_nodes.append( ComposableNode( package="probabilistic_occupancy_grid_map", - plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ ( diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp index 3d02be9ca7156..25eafcd564e2d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp @@ -49,21 +49,23 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMap::OccupancyGridMap( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { } @@ -140,7 +142,7 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r raytraceFreespace(pointcloud, robot_pose); // occupied - MarkCell marker(costmap_, occupancy_cost_value::LETHAL_OBSTACLE); + MarkCell marker(costmap_, cost_value::LETHAL_OBSTACLE); for (PointCloud2ConstIterator iter_x(pointcloud, "x"), iter_y(pointcloud, "y"); iter_x != iter_x.end(); ++iter_x, ++iter_y) { unsigned int mx, my; @@ -155,12 +157,12 @@ void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & r void OccupancyGridMap::updateFreespaceCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::FREE_SPACE); + updateCellsByPointCloud(pointcloud, cost_value::FREE_SPACE); } void OccupancyGridMap::updateOccupiedCells(const PointCloud2 & pointcloud) { - updateCellsByPointCloud(pointcloud, occupancy_cost_value::LETHAL_OBSTACLE); + updateCellsByPointCloud(pointcloud, cost_value::LETHAL_OBSTACLE); } void OccupancyGridMap::updateCellsByPointCloud( @@ -244,9 +246,10 @@ void OccupancyGridMap::raytraceFreespace(const PointCloud2 & pointcloud, const P } constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold - MarkCell marker(costmap_, occupancy_cost_value::FREE_SPACE); + MarkCell marker(costmap_, cost_value::FREE_SPACE); raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); } } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp similarity index 82% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp index 3d176e41583a0..39855ec36260c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp @@ -49,10 +49,10 @@ * David V. Lu!! *********************************************************************/ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -69,25 +69,32 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMapInterface::OccupancyGridMapInterface( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, occupancy_cost_value::NO_INFORMATION) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) { + min_height_ = -std::numeric_limits::infinity(); + max_height_ = std::numeric_limits::infinity(); + resolution_inv_ = 1.0 / resolution_; + offset_initialized_ = false; } -bool OccupancyGridMapInterface::worldToMap( +inline bool OccupancyGridMapInterface::worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my) const { if (wx < origin_x_ || wy < origin_y_) { return false; } - mx = static_cast(std::floor((wx - origin_x_) / resolution_)); - my = static_cast(std::floor((wy - origin_y_) / resolution_)); + mx = static_cast(std::floor((wx - origin_x_) * resolution_inv_)); + my = static_cast(std::floor((wy - origin_y_) * resolution_inv_)); if (mx < size_x_ && my < size_y_) { return true; @@ -229,4 +236,23 @@ void OccupancyGridMapInterface::raytrace( raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); } +void OccupancyGridMapInterface::setHeightLimit(const double min_height, const double max_height) +{ + min_height_ = min_height; + max_height_ = max_height; +} + +void OccupancyGridMapInterface::setFieldOffsets( + const PointCloud2 & input_raw, const PointCloud2 & input_obstacle) +{ + x_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "x")].offset; + y_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "y")].offset; + z_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "z")].offset; + x_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "x")].offset; + y_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "y")].offset; + z_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "z")].offset; + offset_initialized_ = true; +} + } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp similarity index 63% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index d4e177209f99d..c8af97fcf3ba0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -33,6 +33,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -55,21 +58,20 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0); - constexpr double max_angle = autoware::universe_utils::deg2rad(180.0); - constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1); - const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); - - // Transform from base_link to map frame - PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud; // point cloud in map frame - utils::transformPointcloud(raw_pointcloud, robot_pose, map_raw_pointcloud); - utils::transformPointcloud(obstacle_pointcloud, robot_pose, map_obstacle_pointcloud); - - // Transform from map frame to scan frame - PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud; // point cloud in scan frame + const size_t angle_bin_size = + ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/); + + // Transform Matrix from base_link to map frame + mat_map_ = utils::getTransformMatrix(robot_pose); + const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose - utils::transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud); - utils::transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud); + + // Transform Matrix from map frame to scan frame + mat_scan_ = utils::getTransformMatrix(scan2map_pose); + + if (!offset_initialized_) { + setFieldOffsets(raw_pointcloud, obstacle_pointcloud); + } // Create angle bins and sort by distance struct BinInfo @@ -83,41 +85,73 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( double wx; double wy; }; - std::vector> obstacle_pointcloud_angle_bins; - std::vector> raw_pointcloud_angle_bins; - obstacle_pointcloud_angle_bins.resize(angle_bin_size); - raw_pointcloud_angle_bins.resize(angle_bin_size); - for (PointCloud2ConstIterator iter_x(scan_raw_pointcloud, "x"), - iter_y(scan_raw_pointcloud, "y"), iter_wx(map_raw_pointcloud, "x"), - iter_wy(map_raw_pointcloud, "y"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) { - const double angle = atan2(*iter_y, *iter_x); - const int angle_bin_index = (angle - min_angle) / angle_increment; - raw_pointcloud_angle_bins.at(angle_bin_index) - .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy)); + + std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); + std::vector> raw_pointcloud_angle_bins(angle_bin_size); + + const size_t raw_pointcloud_size = raw_pointcloud.width * raw_pointcloud.height; + const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height; + + // Reserve a certain amount of memory in advance for performance reasons + const size_t raw_reserve_size = raw_pointcloud_size / angle_bin_size; + const size_t obstacle_reserve_size = obstacle_pointcloud_size / angle_bin_size; + for (size_t i = 0; i < angle_bin_size; i++) { + raw_pointcloud_angle_bins[i].reserve(raw_reserve_size); + obstacle_pointcloud_angle_bins[i].reserve(obstacle_reserve_size); } + + // Updated every loop inside transformPointAndCalculate() + Eigen::Vector4f pt_map; + int angle_bin_index; + double range; + + size_t global_offset = 0; + for (size_t i = 0; i < raw_pointcloud_size; ++i) { + Eigen::Vector4f pt( + *reinterpret_cast(&raw_pointcloud.data[global_offset + x_offset_raw_]), + *reinterpret_cast(&raw_pointcloud.data[global_offset + y_offset_raw_]), + *reinterpret_cast(&raw_pointcloud.data[global_offset + z_offset_raw_]), 1); + global_offset += raw_pointcloud.point_step; + if (!isPointValid(pt)) { + continue; + } + transformPointAndCalculate(pt, pt_map, angle_bin_index, range); + + raw_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]); + } + for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) { return a.range < b.range; }); } - // create and sort bin for obstacle pointcloud - for (PointCloud2ConstIterator iter_x(scan_obstacle_pointcloud, "x"), - iter_y(scan_obstacle_pointcloud, "y"), iter_wx(map_obstacle_pointcloud, "x"), - iter_wy(map_obstacle_pointcloud, "y"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) { - const double angle = atan2(*iter_y, *iter_x); - const int angle_bin_index = (angle - min_angle) / angle_increment; - const double range = std::hypot(*iter_y, *iter_x); + + // Create obstacle angle bins and sort points by range + global_offset = 0; + for (size_t i = 0; i < obstacle_pointcloud_size; ++i) { + Eigen::Vector4f pt( + *reinterpret_cast( + &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]), + *reinterpret_cast( + &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]), + *reinterpret_cast( + &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]), + 1); + global_offset += obstacle_pointcloud.point_step; + if (!isPointValid(pt)) { + continue; + } + transformPointAndCalculate(pt, pt_map, angle_bin_index, range); + // Ignore obstacle points exceed the range of the raw points if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) { continue; // No raw point in this angle bin } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { continue; // Obstacle point exceeds the range of the raw points } - obstacle_pointcloud_angle_bins.at(angle_bin_index) - .push_back(BinInfo(range, *iter_wx, *iter_wy)); + obstacle_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]); } + for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { std::sort( obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(), @@ -136,7 +170,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } // Second step: Add unknown cell @@ -162,9 +196,8 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); if (!no_freespace_point) { const auto & target = *raw_distance_iter; - raytrace( - source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); } continue; } @@ -177,7 +210,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } else if (no_freespace_point) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } @@ -186,24 +219,23 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( if (next_raw_distance < next_obstacle_point_distance) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); continue; } else { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } } // Third step: Overwrite occupied cell - for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); + for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + const auto & obstacle_point = obstacle_pointcloud_angle_bin.at(dist_index); + setCellValue(obstacle_point.wx, obstacle_point.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -215,7 +247,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= distance_margin_) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -229,3 +261,4 @@ void OccupancyGridMapFixedBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp similarity index 72% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp rename to perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index 3846b3a7d16e7..f5f2c70ae6dfc 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -34,6 +34,9 @@ #endif #include + +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { using sensor_msgs::PointCloud2ConstIterator; @@ -56,26 +59,25 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { - constexpr double min_angle = autoware::universe_utils::deg2rad(-180.0); - constexpr double max_angle = autoware::universe_utils::deg2rad(180.0); - constexpr double angle_increment = autoware::universe_utils::deg2rad(0.1); - const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); + const size_t angle_bin_size = + ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/); // Transform from base_link to map frame - PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud; // point cloud in map frame - utils::transformPointcloud(raw_pointcloud, robot_pose, map_raw_pointcloud); - utils::transformPointcloud(obstacle_pointcloud, robot_pose, map_obstacle_pointcloud); + mat_map_ = utils::getTransformMatrix(robot_pose); - // Transform from map frame to scan frame - PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud; // point cloud in scan frame const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose - utils::transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud); - utils::transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud); + + // Transform from map frame to scan frame + mat_scan_ = utils::getTransformMatrix(scan2map_pose); + + if (!offset_initialized_) { + setFieldOffsets(raw_pointcloud, obstacle_pointcloud); + } // Create angle bins and sort points by range struct BinInfo3D { - BinInfo3D( + explicit BinInfo3D( const double _range = 0.0, const double _wx = 0.0, const double _wy = 0.0, const double _wz = 0.0, const double _projection_length = 0.0, const double _projected_wx = 0.0, const double _projected_wy = 0.0) @@ -97,57 +99,82 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( double projected_wy; }; - std::vector> obstacle_pointcloud_angle_bins; - std::vector> raw_pointcloud_angle_bins; - obstacle_pointcloud_angle_bins.resize(angle_bin_size); - raw_pointcloud_angle_bins.resize(angle_bin_size); - for (PointCloud2ConstIterator iter_x(scan_raw_pointcloud, "x"), - iter_y(scan_raw_pointcloud, "y"), iter_wx(map_raw_pointcloud, "x"), - iter_wy(map_raw_pointcloud, "y"), iter_wz(map_raw_pointcloud, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy, ++iter_wz) { - const double angle = atan2(*iter_y, *iter_x); - const int angle_bin_index = (angle - min_angle) / angle_increment; + std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); + std::vector> raw_pointcloud_angle_bins(angle_bin_size); + + const size_t raw_pointcloud_size = raw_pointcloud.width * raw_pointcloud.height; + const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height; + + // Updated every loop inside transformPointAndCalculate() + Eigen::Vector4f pt_map; + int angle_bin_index; + double range; + + size_t global_offset = 0; + for (size_t i = 0; i < raw_pointcloud_size; i++) { + Eigen::Vector4f pt( + *reinterpret_cast(&raw_pointcloud.data[global_offset + x_offset_raw_]), + *reinterpret_cast(&raw_pointcloud.data[global_offset + y_offset_raw_]), + *reinterpret_cast(&raw_pointcloud.data[global_offset + z_offset_raw_]), 1); + global_offset += raw_pointcloud.point_step; + if (!isPointValid(pt)) { + continue; + } + transformPointAndCalculate(pt, pt_map, angle_bin_index, range); + raw_pointcloud_angle_bins.at(angle_bin_index) - .emplace_back(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy, *iter_wz); + .emplace_back(range, pt_map[0], pt_map[1], pt_map[2]); } + for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) { return a.range < b.range; }); } + // Create obstacle angle bins and sort points by range - for (PointCloud2ConstIterator iter_x(scan_obstacle_pointcloud, "x"), - iter_y(scan_obstacle_pointcloud, "y"), iter_z(scan_obstacle_pointcloud, "z"), - iter_wx(map_obstacle_pointcloud, "x"), iter_wy(map_obstacle_pointcloud, "y"), - iter_wz(map_obstacle_pointcloud, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_wx, ++iter_wy, ++iter_wz) { + global_offset = 0; + for (size_t i = 0; i < obstacle_pointcloud_size; i++) { + Eigen::Vector4f pt( + *reinterpret_cast( + &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]), + *reinterpret_cast( + &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]), + *reinterpret_cast( + &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]), + 1); + global_offset += obstacle_pointcloud.point_step; + if (!isPointValid(pt)) { + continue; + } + transformPointAndCalculate(pt, pt_map, angle_bin_index, range); const double scan_z = scan_origin.position.z - robot_pose.position.z; - const double obstacle_z = (*iter_wz) - robot_pose.position.z; + const double obstacle_z = (pt_map[2]) - robot_pose.position.z; const double dz = scan_z - obstacle_z; - const double angle = atan2(*iter_y, *iter_x); - const int angle_bin_index = (angle - min_angle) / angle_increment; - const double range = std::hypot(*iter_x, *iter_y); + // Ignore obstacle points exceed the range of the raw points if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) { continue; // No raw point in this angle bin } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { continue; // Obstacle point exceeds the range of the raw points } + if (dz > projection_dz_threshold_) { const double ratio = obstacle_z / dz; const double projection_length = range * ratio; - const double projected_wx = (*iter_wx) + ((*iter_wx) - scan_origin.position.x) * ratio; - const double projected_wy = (*iter_wy) + ((*iter_wy) - scan_origin.position.y) * ratio; + const double projected_wx = (pt_map[0]) + ((pt_map[0]) - scan_origin.position.x) * ratio; + const double projected_wy = (pt_map[1]) + ((pt_map[1]) - scan_origin.position.y) * ratio; obstacle_pointcloud_angle_bins.at(angle_bin_index) .emplace_back( - range, *iter_wx, *iter_wy, *iter_wz, projection_length, projected_wx, projected_wy); + range, pt_map[0], pt_map[1], pt_map[2], projection_length, projected_wx, projected_wy); } else { obstacle_pointcloud_angle_bins.at(angle_bin_index) .emplace_back( - range, *iter_wx, *iter_wy, *iter_wz, std::numeric_limits::infinity(), + range, pt_map[0], pt_map[1], pt_map[2], std::numeric_limits::infinity(), std::numeric_limits::infinity(), std::numeric_limits::infinity()); } } + for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { std::sort( obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(), @@ -192,7 +219,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( } raytrace( scan_origin.position.x, scan_origin.position.y, ray_end.wx, ray_end.wy, - occupancy_cost_value::FREE_SPACE); + cost_value::FREE_SPACE); } if (pub_debug_grid_) @@ -219,7 +246,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); break; } @@ -227,7 +254,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); raytrace( source.wx, source.wy, source.projected_wx, source.projected_wy, - occupancy_cost_value::NO_INFORMATION); + cost_value::NO_INFORMATION); continue; } @@ -243,13 +270,13 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_raw_distance < next_obstacle_point_distance) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, occupancy_cost_value::FREE_SPACE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); + setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); continue; } else { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::NO_INFORMATION); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); continue; } } @@ -258,11 +285,10 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (pub_debug_grid_) converter.addLayerFromCostmap2D(*this, "added_unknown", debug_grid_); // Third step: Overwrite occupied cell - for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); + for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); + const auto & obstacle_point = obstacle_pointcloud_angle_bin.at(dist_index); + setCellValue(obstacle_point.wx, obstacle_point.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; @@ -274,7 +300,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (next_obstacle_point_distance <= obstacle_separation_threshold_) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, occupancy_cost_value::LETHAL_OBSTACLE); + raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); continue; } } @@ -299,3 +325,4 @@ void OccupancyGridMapProjectiveBlindSpot::initRosParam(rclcpp::Node & node) } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp similarity index 96% rename from perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp rename to perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp index 6ac681eff4916..1e88b7a39fd14 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +namespace autoware::occupancy_grid_map +{ namespace fusion_policy { @@ -61,9 +63,9 @@ namespace overwrite_fusion */ State getApproximateState(const unsigned char & occupancy) { - if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + if (occupancy >= cost_value::OCCUPIED_THRESHOLD) { return State::OCCUPIED; - } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + } else if (occupancy <= cost_value::FREE_THRESHOLD) { return State::FREE; } else { return State::UNKNOWN; @@ -171,7 +173,7 @@ struct dempsterShaferOccupancy } // initialize with probability - dempsterShaferOccupancy(double occupied_probability) + explicit dempsterShaferOccupancy(double occupied_probability) { // confine to [0, 1] double p = std::max(0.0, std::min(1.0, occupied_probability)); @@ -320,3 +322,4 @@ unsigned char singleFrameOccupancyFusion( } } // namespace fusion_policy +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp index 40f538a64f6e9..74506d095b36d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -48,15 +50,15 @@ inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( float pz{}; float not_pz{}; float po_hat{}; - if (z == occupancy_cost_value::LETHAL_OBSTACLE) { + if (z == cost_value::LETHAL_OBSTACLE) { pz = probability_matrix_(Index::OCCUPIED, Index::OCCUPIED); not_pz = probability_matrix_(Index::FREE, Index::OCCUPIED); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); - } else if (z == occupancy_cost_value::FREE_SPACE) { + } else if (z == cost_value::FREE_SPACE) { pz = 1.f - probability_matrix_(Index::FREE, Index::FREE); not_pz = 1.f - probability_matrix_(Index::OCCUPIED, Index::FREE); po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); - } else if (z == occupancy_cost_value::NO_INFORMATION) { + } else if (z == cost_value::NO_INFORMATION) { const float inv_v_ratio = 1.f / v_ratio_; po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); } @@ -79,3 +81,4 @@ bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy } } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp rename to perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp index 607694d6da571..9f3c3c7e40eaf 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // cspell: ignore LOBF +namespace autoware::occupancy_grid_map +{ namespace costmap_2d { @@ -35,7 +37,7 @@ inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( using fusion_policy::convertProbabilityToChar; using fusion_policy::log_odds_fusion::logOddsFusion; - constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown = cost_value::NO_INFORMATION; constexpr unsigned char unknown_margin = 1; /* Tau and ST decides how fast the observation decay to the unknown status*/ constexpr double tau = 0.75; @@ -68,4 +70,6 @@ bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupanc } return true; } + } // namespace costmap_2d +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp similarity index 92% rename from perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp rename to perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 209207fe10f34..902de2148cffe 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include +namespace autoware::occupancy_grid_map +{ namespace utils { @@ -58,6 +60,13 @@ void transformPointcloud( output.header.frame_id = ""; } +Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose) +{ + auto transform = autoware::universe_utils::pose2transform(pose); + Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast(); + return tf_matrix; +} + bool cropPointcloudByHeight( const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame, const float min_height, const float max_height, @@ -189,13 +198,14 @@ bool extractCommonPointCloud( */ unsigned char getApproximateOccupancyState(const unsigned char & value) { - if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { - return occupancy_cost_value::LETHAL_OBSTACLE; - } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { - return occupancy_cost_value::FREE_SPACE; + if (value >= cost_value::OCCUPIED_THRESHOLD) { + return cost_value::LETHAL_OBSTACLE; + } else if (value <= cost_value::FREE_THRESHOLD) { + return cost_value::FREE_SPACE; } else { - return occupancy_cost_value::NO_INFORMATION; + return cost_value::NO_INFORMATION; } } } // namespace utils +} // namespace autoware::occupancy_grid_map diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index d8ae702ef60ca..d8647ae2ba744 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -13,9 +13,9 @@ ament_cmake_auto autoware_cmake + eigen3_cmake_module autoware_universe_utils - eigen3_cmake_module grid_map_costmap_2d grid_map_msgs grid_map_ros @@ -27,12 +27,9 @@ rclcpp rclcpp_components sensor_msgs - tf2 tf2_eigen - tf2_geometry_msgs tf2_ros tf2_sensor_msgs - visualization_msgs pointcloud_preprocessor pointcloud_to_laserscan diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index fa93db802b0ce..a271e17a1e566 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" +#include "synchronized_grid_map_fusion_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; using costmap_2d::OccupancyGridMapLOBFUpdater; @@ -404,7 +404,7 @@ nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; costmap2d.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } @@ -424,7 +424,7 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { const unsigned int index = i + j * occupancy_grid_map.info.width; gridmap.setCost( - i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + i, j, cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); } } return gridmap; @@ -453,14 +453,14 @@ nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsg msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); - unsigned char * data = occupancy_grid_map.getCharMap(); + const unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::GridMapFusionNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp similarity index 85% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 84ca13c8b1881..c839f160aab9b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -45,7 +45,7 @@ // cspell: ignore LOBF -namespace synchronized_grid_map_fusion +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -124,6 +124,6 @@ class GridMapFusionNode : public rclcpp::Node fusion_policy::FusionMethod fusion_method_; }; -} // namespace synchronized_grid_map_fusion +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index 777c180fe1a3a..6c4ad8135b37d 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" +#include "laserscan_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -35,7 +35,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMap; using costmap_2d::OccupancyGridMapBBFUpdater; @@ -229,12 +229,12 @@ OccupancyGrid::UniquePtr LaserscanBasedOccupancyGridMapNode::OccupancyGridMapToM unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 82% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index a8adea6e700e5..a599f7b564c8b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -36,7 +36,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapUpdaterInterface; @@ -100,6 +100,6 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node bool enable_single_frame_mode_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index d019280aefda0..0d772e73c1ad8 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" +#include "pointcloud_based_occupancy_grid_map_node.hpp" -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -37,10 +37,11 @@ #endif #include +#include #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using costmap_2d::OccupancyGridMapBBFUpdater; using costmap_2d::OccupancyGridMapFixedBlindSpot; @@ -141,33 +142,46 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( scan_origin_frame_ = input_raw_msg->header.frame_id; } - // Apply height filter - PointCloud2 cropped_obstacle_pc{}; - PointCloud2 cropped_raw_pc{}; + PointCloud2 trans_input_raw{}, trans_input_obstacle{}; + bool is_raw_transformed = false; + bool is_obstacle_transformed = false; + + // Prepare for applying height filter if (use_height_filter_) { - if (!utils::cropPointcloudByHeight( - *input_obstacle_msg, *tf2_, base_link_frame_, min_height_, max_height_, - cropped_obstacle_pc)) { - return; + // Make sure that the frame is base_link + if (input_raw_msg->header.frame_id != base_link_frame_) { + if (!utils::transformPointcloud(*input_raw_msg, *tf2_, base_link_frame_, trans_input_raw)) { + return; + } + is_raw_transformed = true; } - if (!utils::cropPointcloudByHeight( - *input_raw_msg, *tf2_, base_link_frame_, min_height_, max_height_, cropped_raw_pc)) { - return; + if (input_obstacle_msg->header.frame_id != base_link_frame_) { + if (!utils::transformPointcloud( + *input_obstacle_msg, *tf2_, base_link_frame_, trans_input_obstacle)) { + return; + } + is_obstacle_transformed = true; } + occupancy_grid_map_ptr_->setHeightLimit(min_height_, max_height_); + } else { + occupancy_grid_map_ptr_->setHeightLimit( + -std::numeric_limits::infinity(), std::numeric_limits::infinity()); } - const PointCloud2 & filtered_obstacle_pc = - use_height_filter_ ? cropped_obstacle_pc : *input_obstacle_msg; - const PointCloud2 & filtered_raw_pc = use_height_filter_ ? cropped_raw_pc : *input_raw_msg; + + const PointCloud2::ConstSharedPtr input_raw_use = + is_raw_transformed ? std::make_shared(trans_input_raw) : input_raw_msg; + const PointCloud2::ConstSharedPtr input_obstacle_use = + is_obstacle_transformed ? std::make_shared(trans_input_obstacle) + : input_obstacle_msg; // Filter obstacle pointcloud by raw pointcloud - PointCloud2 filtered_obstacle_pc_common{}; + PointCloud2 input_obstacle_pc_common{}; + bool use_input_obstacle_pc_common = false; if (filter_obstacle_pointcloud_by_raw_pointcloud_) { - if (!utils::extractCommonPointCloud( - filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)) { - filtered_obstacle_pc_common = filtered_obstacle_pc; + if (utils::extractCommonPointCloud( + *input_obstacle_use, *input_raw_use, input_obstacle_pc_common)) { + use_input_obstacle_pc_common = true; } - } else { - filtered_obstacle_pc_common = filtered_obstacle_pc; } // Get from map to sensor frame pose @@ -191,7 +205,8 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( gridmap_origin.position.x - occupancy_grid_map_ptr_->getSizeInMetersX() / 2, gridmap_origin.position.y - occupancy_grid_map_ptr_->getSizeInMetersY() / 2); occupancy_grid_map_ptr_->updateWithPointCloud( - filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin); + *input_raw_use, (use_input_obstacle_pc_common ? input_obstacle_pc_common : *input_obstacle_use), + robot_pose, scan_origin); if (enable_single_frame_mode_) { // publish @@ -250,12 +265,12 @@ OccupancyGrid::UniquePtr PointcloudBasedOccupancyGridMapNode::OccupancyGridMapTo unsigned char * data = occupancy_grid_map.getCharMap(); for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { - msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + msg_ptr->data[i] = cost_value::cost_translation_table[data[i]]; } return msg_ptr; } -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map #include -RCLCPP_COMPONENTS_REGISTER_NODE(occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode) diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 81% rename from perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 1d119102dc28d..e0c7ef74988f4 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include #include @@ -38,7 +38,7 @@ #include #include -namespace occupancy_grid_map +namespace autoware::occupancy_grid_map { using builtin_interfaces::msg::Time; using costmap_2d::OccupancyGridMapInterface; @@ -93,6 +93,6 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node bool filter_obstacle_pointcloud_by_raw_pointcloud_; }; -} // namespace occupancy_grid_map +} // namespace autoware::occupancy_grid_map -#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp index c41c809a92b0c..f03852562f7ca 100644 --- a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the CostTranslationTable and InverseCostTranslationTable functions -using occupancy_cost_value::cost_translation_table; -using occupancy_cost_value::inverse_cost_translation_table; +using autoware::occupancy_grid_map::cost_value::cost_translation_table; +using autoware::occupancy_grid_map::cost_value::inverse_cost_translation_table; TEST(CostTranslationTableTest, TestRange) { diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp index 6b3dc8a2ebcef..78137371c720e 100644 --- a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp" + +#include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" #include // Test the log-odds update rule TEST(FusionPolicyTest, TestLogOddsUpdateRule) { - using fusion_policy::log_odds_fusion::logOddsFusion; + using autoware::occupancy_grid_map::fusion_policy::log_odds_fusion::logOddsFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; @@ -49,7 +50,7 @@ TEST(FusionPolicyTest, TestLogOddsUpdateRule) // Test the dempster-shafer update rule TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) { - using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + using autoware::occupancy_grid_map::fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; const double MARGIN = 0.03; const double OCCUPIED = 1.0 - MARGIN; const double FREE = 0.0 + MARGIN; diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 4bc3dca0a67bd..17df981d849d4 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -13,7 +13,7 @@ // limitations under the License. // autoware -#include "probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include @@ -77,14 +77,14 @@ TEST(TestUtils, TestCropPointcloudByHeight) sensor_msgs::msg::PointCloud2 test1_output, test2_output, test3_output; // case1: normal input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); // case2: normal input, empty output - EXPECT_NO_THROW(utils::cropPointcloudByHeight( + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( ros_cloud_10, mock_buffer, "base_link", -2.0, -0.1, test2_output)); // case3: empty input, normal output - EXPECT_NO_THROW( - utils::cropPointcloudByHeight(ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); + EXPECT_NO_THROW(autoware::occupancy_grid_map::utils::cropPointcloudByHeight( + ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); } diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 527c565b91e05..21517f8c0f766 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -16,7 +16,7 @@ set(SHAPE_ESTIMATION_DEPENDENCIES Eigen3 ) -ament_auto_add_library(shape_estimation_lib SHARED +ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/shape_estimator.cpp lib/model/bounding_box.cpp lib/model/convex_hull.cpp @@ -27,23 +27,23 @@ ament_auto_add_library(shape_estimation_lib SHARED lib/filter/trailer_filter.cpp lib/filter/no_filter.cpp lib/filter/utils.cpp - lib/corrector/no_corrector.cpp lib/corrector/utils.cpp + lib/corrector/no_corrector.cpp ) -ament_target_dependencies(shape_estimation_lib ${SHAPE_ESTIMATION_DEPENDENCIES}) +ament_target_dependencies(${PROJECT_NAME}_lib ${SHAPE_ESTIMATION_DEPENDENCIES}) -target_include_directories(shape_estimation_lib +target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC "${PCL_INCLUDE_DIRS}" "${EIGEN3_INCLUDE_DIR}" ) -ament_auto_add_library(shape_estimation_node SHARED - src/node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/shape_estimation_node.cpp ) -target_include_directories(shape_estimation_node +target_include_directories(${PROJECT_NAME} PUBLIC $ $ @@ -51,15 +51,15 @@ target_include_directories(shape_estimation_node ${CMAKE_CURRENT_SOURCE_DIR}/src ) -ament_target_dependencies(shape_estimation_node ${SHAPE_ESTIMATION_DEPENDENCIES}) +ament_target_dependencies(${PROJECT_NAME} ${SHAPE_ESTIMATION_DEPENDENCIES}) -target_link_libraries(shape_estimation_node +target_link_libraries(${PROJECT_NAME} shape_estimation_lib ) -rclcpp_components_register_node(shape_estimation_node - PLUGIN "ShapeEstimationNode" - EXECUTABLE shape_estimation +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::shape_estimation::ShapeEstimationNode" + EXECUTABLE shape_estimation_node ) ament_auto_package(INSTALL_TO_SHARE @@ -79,6 +79,6 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_shape_estimation ${test_files}) target_link_libraries(test_shape_estimation - shape_estimation_node + ${PROJECT_NAME} ) endif() diff --git a/perception/shape_estimation/README.md b/perception/shape_estimation/README.md index 206e8d422c727..9615b314f62c6 100644 --- a/perception/shape_estimation/README.md +++ b/perception/shape_estimation/README.md @@ -36,7 +36,7 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull ## Parameters -{{ json_to_markdown("perception/shape_estimation/schema/shape_estimation.schema.json") }} +{{ json_to_markdown("perception/autoware/shape_estimation/schema/shape_estimation.schema.json") }} ## Assumptions / Known limits diff --git a/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp index ec1a2b8a42973..f8dc489a563be 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class BicycleCorrector : public VehicleCorrector { public: @@ -37,4 +42,7 @@ class BicycleCorrector : public VehicleCorrector ~BicycleCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp similarity index 73% rename from perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp index 4ef1f00122ca4..d7386ace5b364 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/bus_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" - +namespace autoware::shape_estimation +{ +namespace corrector +{ class BusCorrector : public VehicleCorrector { public: @@ -36,4 +39,7 @@ class BusCorrector : public VehicleCorrector ~BusCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__BUS_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp index d4d33599c7730..7292704d0f6d9 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/car_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class CarCorrector : public VehicleCorrector { public: @@ -36,4 +41,7 @@ class CarCorrector : public VehicleCorrector ~CarCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CAR_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp new file mode 100644 index 0000000000000..dd4c4f68d3f46 --- /dev/null +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp @@ -0,0 +1,27 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ + +#include "autoware/shape_estimation/corrector/bicycle_corrector.hpp" +#include "autoware/shape_estimation/corrector/bus_corrector.hpp" +#include "autoware/shape_estimation/corrector/car_corrector.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/corrector/no_corrector.hpp" +#include "autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp" +#include "autoware/shape_estimation/corrector/trailer_corrector.hpp" +#include "autoware/shape_estimation/corrector/truck_corrector.hpp" + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp similarity index 73% rename from perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp index 1ce1273f042b9..1dcf85b46887c 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ #include #include #include - +namespace autoware::shape_estimation +{ +namespace corrector +{ class ShapeEstimationCorrectorInterface { public: @@ -31,4 +34,7 @@ class ShapeEstimationCorrectorInterface autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) = 0; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp similarity index 58% rename from perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp index 1303f6f859191..e3c33100861a0 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/no_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class NoCorrector : public ShapeEstimationCorrectorInterface { public: @@ -26,7 +31,14 @@ class NoCorrector : public ShapeEstimationCorrectorInterface ~NoCorrector() {} bool correct( - autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) override; + [[maybe_unused]] autoware_perception_msgs::msg::Shape & shape, + [[maybe_unused]] geometry_msgs::msg::Pose & pose) override + { + return true; + } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__NO_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp index 0d1740dc30d61..a05027a3348b6 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/reference_shape_size_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp @@ -12,13 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class ReferenceShapeBasedVehicleCorrector : public ShapeEstimationCorrectorInterface { ReferenceShapeSizeInfo ref_shape_size_info_; @@ -38,4 +43,6 @@ class ReferenceShapeBasedVehicleCorrector : public ShapeEstimationCorrectorInter } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__REFERENCE_SHAPE_SIZE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp similarity index 77% rename from perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp index 6dd885353c78f..afd0f1f7c451d 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + // Generally speaking, trailer would be much larger than bus and truck. // But currently we do not make large differences among bus/truck/trailer // because current our vehicle classification is not reliable enough. @@ -40,4 +45,7 @@ class TrailerCorrector : public VehicleCorrector ~TrailerCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp index 3b2475286fa61..8a641e84176a5 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ -#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "autoware/shape_estimation/corrector/vehicle_corrector.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class TruckCorrector : public VehicleCorrector { public: @@ -37,4 +42,7 @@ class TruckCorrector : public VehicleCorrector ~TruckCorrector() = default; }; -#endif // SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__TRUCK_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp similarity index 82% rename from perception/shape_estimation/include/shape_estimation/corrector/utils.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp index 951f3d16aa278..a16cb146ca4a3 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include +namespace autoware::shape_estimation +{ + namespace corrector_utils { struct CorrectionBBParameters @@ -46,4 +49,6 @@ bool correctWithReferenceYaw( } // namespace corrector_utils -#endif // SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__UTILS_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp b/perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp similarity index 75% rename from perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp index 1b38f15620663..e1b292eb5318a 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/vehicle_corrector.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp @@ -12,13 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace corrector +{ + class VehicleCorrector : public ShapeEstimationCorrectorInterface { protected: @@ -47,4 +52,7 @@ class VehicleCorrector : public ShapeEstimationCorrectorInterface void setParams(const corrector_utils::CorrectionBBParameters & params) { params_ = params; } }; -#endif // SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ +} // namespace corrector +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__CORRECTOR__VEHICLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp index 301921b00d439..54d45498a3d40 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/bus_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class BusFilter : public ShapeEstimationFilterInterface { public: @@ -30,4 +34,7 @@ class BusFilter : public ShapeEstimationFilterInterface const geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__BUS_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp index 79104823bb7ab..8ff79f901d8a3 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/car_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class CarFilter : public ShapeEstimationFilterInterface { public: @@ -30,4 +34,7 @@ class CarFilter : public ShapeEstimationFilterInterface const geometry_msgs::msg::Pose & pose) override; }; -#endif // SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__CAR_FILTER_HPP_ diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp new file mode 100644 index 0000000000000..5b7bad4f57208 --- /dev/null +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp @@ -0,0 +1,25 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ + +#include "autoware/shape_estimation/filter/bus_filter.hpp" +#include "autoware/shape_estimation/filter/car_filter.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/no_filter.hpp" +#include "autoware/shape_estimation/filter/trailer_filter.hpp" +#include "autoware/shape_estimation/filter/truck_filter.hpp" + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp similarity index 75% rename from perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp index 67dfe27eb93b0..cba7715314a9e 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/filter_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp @@ -12,14 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ #include #include #include +namespace autoware::shape_estimation +{ +namespace filter +{ + class ShapeEstimationFilterInterface { public: @@ -31,4 +36,7 @@ class ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) = 0; }; -#endif // SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__FILTER_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp similarity index 58% rename from perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp index d007e6c61c965..f05c0e9affa02 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/no_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ class NoFilter : public ShapeEstimationFilterInterface { @@ -25,8 +30,14 @@ class NoFilter : public ShapeEstimationFilterInterface ~NoFilter() = default; bool filter( - const autoware_perception_msgs::msg::Shape & shape, - const geometry_msgs::msg::Pose & pose) override; + [[maybe_unused]] const autoware_perception_msgs::msg::Shape & shape, + [[maybe_unused]] const geometry_msgs::msg::Pose & pose) override + { + return true; + } }; -#endif // SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ +} // namespace filter +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__NO_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp index 319d3e25c77c0..21394938a8026 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp @@ -12,12 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ class TrailerFilter : public ShapeEstimationFilterInterface { public: @@ -29,5 +33,7 @@ class TrailerFilter : public ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) override; }; +} // namespace filter +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp similarity index 70% rename from perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp index e67da1c31115e..a941ec4fd7932 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/truck_filter.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ -#include "shape_estimation/filter/filter_interface.hpp" +#include "autoware/shape_estimation/filter/filter_interface.hpp" #include "utils.hpp" - +namespace autoware::shape_estimation +{ +namespace filter +{ class TruckFilter : public ShapeEstimationFilterInterface { public: @@ -29,5 +32,7 @@ class TruckFilter : public ShapeEstimationFilterInterface const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) override; }; +} // namespace filter +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__TRUCK_FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/utils.hpp b/perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp similarity index 77% rename from perception/shape_estimation/include/shape_estimation/filter/utils.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp index 8d487b8d42a93..7f56664e0a57d 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/utils.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__FILTER__UTILS_HPP_ -#define SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ #include +namespace autoware::shape_estimation +{ namespace utils { @@ -24,4 +26,6 @@ bool filterVehicleBoundingBox( const float max_length); } // namespace utils -#endif // SHAPE_ESTIMATION__FILTER__UTILS_HPP_ +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__FILTER__UTILS_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp similarity index 80% rename from perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp index 2ac179d030629..c9d6889c21de5 100644 --- a/perception/shape_estimation/include/shape_estimation/model/bounding_box.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp @@ -12,14 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ -#define SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ -#include "shape_estimation/model/model_interface.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include +namespace autoware::shape_estimation +{ +namespace model +{ + class BoundingBoxShapeModel : public ShapeEstimationModelInterface { private: @@ -47,4 +52,7 @@ class BoundingBoxShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__BOUNDING_BOX_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp similarity index 71% rename from perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp index 2748d4a53a807..c3ffa297833fb 100644 --- a/perception/shape_estimation/include/shape_estimation/model/convex_hull.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ -#define SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace model +{ class ConvexHullShapeModel : public ShapeEstimationModelInterface { @@ -30,4 +35,7 @@ class ConvexHullShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__CONVEX_HULL_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/cylinder.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp similarity index 74% rename from perception/shape_estimation/include/shape_estimation/model/cylinder.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp index ccd28afa3173b..6623934cdcc21 100644 --- a/perception/shape_estimation/include/shape_estimation/model/cylinder.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ -#define SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" + +namespace autoware::shape_estimation +{ +namespace model +{ class CylinderShapeModel : public ShapeEstimationModelInterface { @@ -33,4 +38,7 @@ class CylinderShapeModel : public ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) override; }; -#endif // SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__CYLINDER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/model.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp similarity index 60% rename from perception/shape_estimation/include/shape_estimation/model/model.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp index 65080ac884073..382812de6f188 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__MODEL_HPP_ -#define SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ -#include "shape_estimation/model/bounding_box.hpp" -#include "shape_estimation/model/convex_hull.hpp" -#include "shape_estimation/model/cylinder.hpp" -#include "shape_estimation/model/model_interface.hpp" +#include "autoware/shape_estimation/model/bounding_box.hpp" +#include "autoware/shape_estimation/model/convex_hull.hpp" +#include "autoware/shape_estimation/model/cylinder.hpp" +#include "autoware/shape_estimation/model/model_interface.hpp" -#endif // SHAPE_ESTIMATION__MODEL__MODEL_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp b/perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp similarity index 78% rename from perception/shape_estimation/include/shape_estimation/model/model_interface.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp index f5324196f69bd..380e88704baae 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ -#define SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ #include #include @@ -24,6 +24,11 @@ #include +namespace autoware::shape_estimation +{ +namespace model +{ + class ShapeEstimationModelInterface { public: @@ -37,4 +42,7 @@ class ShapeEstimationModelInterface geometry_msgs::msg::Pose & pose_output) = 0; }; -#endif // SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ +} // namespace model +} // namespace autoware::shape_estimation + +#endif // AUTOWARE__SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp b/perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp similarity index 90% rename from perception/shape_estimation/include/shape_estimation/shape_estimator.hpp rename to perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp index 7cdab6f86b585..9759c7c77855e 100644 --- a/perception/shape_estimation/include/shape_estimation/shape_estimator.hpp +++ b/perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ -#define SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#ifndef AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ #include #include @@ -27,6 +27,9 @@ #include +namespace autoware::shape_estimation +{ + struct ReferenceYawInfo { float yaw; @@ -75,5 +78,6 @@ class ShapeEstimator const boost::optional & ref_shape_size_info, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output); }; +} // namespace autoware::shape_estimation -#endif // SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ +#endif // AUTOWARE__SHAPE_ESTIMATION__SHAPE_ESTIMATOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp deleted file mode 100644 index e4efc17181e52..0000000000000 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright 2018 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ -#define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ - -#include "shape_estimation/corrector/bicycle_corrector.hpp" -#include "shape_estimation/corrector/bus_corrector.hpp" -#include "shape_estimation/corrector/car_corrector.hpp" -#include "shape_estimation/corrector/corrector_interface.hpp" -#include "shape_estimation/corrector/no_corrector.hpp" -#include "shape_estimation/corrector/reference_shape_size_corrector.hpp" -#include "shape_estimation/corrector/trailer_corrector.hpp" -#include "shape_estimation/corrector/truck_corrector.hpp" - -#endif // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp b/perception/shape_estimation/include/shape_estimation/filter/filter.hpp deleted file mode 100644 index b205cbd6791ba..0000000000000 --- a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 Autoware Foundation. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SHAPE_ESTIMATION__FILTER__FILTER_HPP_ -#define SHAPE_ESTIMATION__FILTER__FILTER_HPP_ - -#include "shape_estimation/filter/bus_filter.hpp" -#include "shape_estimation/filter/car_filter.hpp" -#include "shape_estimation/filter/filter_interface.hpp" -#include "shape_estimation/filter/no_filter.hpp" -#include "shape_estimation/filter/trailer_filter.hpp" -#include "shape_estimation/filter/truck_filter.hpp" - -#endif // SHAPE_ESTIMATION__FILTER__FILTER_HPP_ diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index f13b5d1e5f273..88cea81412d97 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/perception/shape_estimation/lib/corrector/no_corrector.cpp b/perception/shape_estimation/lib/corrector/no_corrector.cpp index e18de4acefcdd..b37ee289871dc 100644 --- a/perception/shape_estimation/lib/corrector/no_corrector.cpp +++ b/perception/shape_estimation/lib/corrector/no_corrector.cpp @@ -12,11 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/no_corrector.hpp" - -bool NoCorrector::correct( - [[maybe_unused]] autoware_perception_msgs::msg::Shape & shape_output, - [[maybe_unused]] geometry_msgs::msg::Pose & pose_output) -{ - return true; -} +#include "autoware/shape_estimation/corrector/no_corrector.hpp" diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index 1332647b0f1f4..a18e09d5b8a04 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/utils.hpp" +#define EIGEN_MPL2_ONLY + +#include "autoware/shape_estimation/corrector/utils.hpp" -#include +#include "autoware/universe_utils/geometry/geometry.hpp" #include #include @@ -28,12 +30,14 @@ #include #endif +#include +#include + #include #include -#define EIGEN_MPL2_ONLY -#include -#include +namespace autoware::shape_estimation +{ namespace corrector_utils { @@ -398,3 +402,5 @@ bool correctWithReferenceYawAndShapeSize( return true; } } // namespace corrector_utils + +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/bus_filter.cpp b/perception/shape_estimation/lib/filter/bus_filter.cpp index 536545813237f..a301ce716cca2 100644 --- a/perception/shape_estimation/lib/filter/bus_filter.cpp +++ b/perception/shape_estimation/lib/filter/bus_filter.cpp @@ -12,8 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/bus_filter.hpp" - +#include "autoware/shape_estimation/filter/bus_filter.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ bool BusFilter::filter( const autoware_perception_msgs::msg::Shape & shape, [[maybe_unused]] const geometry_msgs::msg::Pose & pose) @@ -23,3 +26,6 @@ bool BusFilter::filter( constexpr float max_length = 17.0; return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/car_filter.cpp b/perception/shape_estimation/lib/filter/car_filter.cpp index e12b9c211a24e..ab7a8d69d109f 100644 --- a/perception/shape_estimation/lib/filter/car_filter.cpp +++ b/perception/shape_estimation/lib/filter/car_filter.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/car_filter.hpp" +#include "autoware/shape_estimation/filter/car_filter.hpp" +namespace autoware::shape_estimation +{ +namespace filter +{ bool CarFilter::filter( const autoware_perception_msgs::msg::Shape & shape, [[maybe_unused]] const geometry_msgs::msg::Pose & pose) @@ -23,3 +27,6 @@ bool CarFilter::filter( constexpr float max_length = 5.8; return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/no_filter.cpp b/perception/shape_estimation/lib/filter/no_filter.cpp index 564de63461c4f..22690be2ed769 100644 --- a/perception/shape_estimation/lib/filter/no_filter.cpp +++ b/perception/shape_estimation/lib/filter/no_filter.cpp @@ -12,11 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/no_filter.hpp" - -bool NoFilter::filter( - [[maybe_unused]] const autoware_perception_msgs::msg::Shape & shape, - [[maybe_unused]] const geometry_msgs::msg::Pose & pose) -{ - return true; -} +#include "autoware/shape_estimation/filter/no_filter.hpp" diff --git a/perception/shape_estimation/lib/filter/trailer_filter.cpp b/perception/shape_estimation/lib/filter/trailer_filter.cpp index 5087350f4d60a..378ce085aa881 100644 --- a/perception/shape_estimation/lib/filter/trailer_filter.cpp +++ b/perception/shape_estimation/lib/filter/trailer_filter.cpp @@ -12,7 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/trailer_filter.hpp" +#include "autoware/shape_estimation/filter/trailer_filter.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ bool TrailerFilter::filter( const autoware_perception_msgs::msg::Shape & shape, @@ -23,3 +28,6 @@ bool TrailerFilter::filter( constexpr float max_length = 25.0; // maximum Full-TRAILER size in JAPAN(normally upto 18m) return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/truck_filter.cpp b/perception/shape_estimation/lib/filter/truck_filter.cpp index ad733bb58c97c..4afcccaf55347 100644 --- a/perception/shape_estimation/lib/filter/truck_filter.cpp +++ b/perception/shape_estimation/lib/filter/truck_filter.cpp @@ -12,7 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/truck_filter.hpp" +#include "autoware/shape_estimation/filter/truck_filter.hpp" + +namespace autoware::shape_estimation +{ +namespace filter +{ bool TruckFilter::filter( const autoware_perception_msgs::msg::Shape & shape, @@ -23,3 +28,6 @@ bool TruckFilter::filter( constexpr float max_length = 18.0; // upto 12m in japanese law return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } + +} // namespace filter +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/filter/utils.cpp b/perception/shape_estimation/lib/filter/utils.cpp index 9412be4e99a9e..616eb5e22769e 100644 --- a/perception/shape_estimation/lib/filter/utils.cpp +++ b/perception/shape_estimation/lib/filter/utils.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/filter/utils.hpp" +#include "autoware/shape_estimation/filter/utils.hpp" +namespace autoware::shape_estimation +{ namespace utils { @@ -43,4 +45,6 @@ bool filterVehicleBoundingBox( } return true; } + } // namespace utils +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/bounding_box.cpp b/perception/shape_estimation/lib/model/bounding_box.cpp index a6693fa56d55b..df751be27d8c0 100644 --- a/perception/shape_estimation/lib/model/bounding_box.cpp +++ b/perception/shape_estimation/lib/model/bounding_box.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/bounding_box.hpp" +#define EIGEN_MPL2_ONLY + +#include "autoware/shape_estimation/model/bounding_box.hpp" #include #include #include -#include +#include "autoware_perception_msgs/msg/shape.hpp" #include @@ -33,14 +35,17 @@ #include #endif +#include + #include #include #include #include -#define EIGEN_MPL2_ONLY - -#include +namespace autoware::shape_estimation +{ +namespace model +{ constexpr float epsilon = 0.001; @@ -252,3 +257,6 @@ float BoundingBoxShapeModel::boostOptimize( float theta_star = min.first; return theta_star; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/convex_hull.cpp b/perception/shape_estimation/lib/model/convex_hull.cpp index d29fb6aa90a99..3c68634aaff4c 100644 --- a/perception/shape_estimation/lib/model/convex_hull.cpp +++ b/perception/shape_estimation/lib/model/convex_hull.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/convex_hull.hpp" +#include "autoware/shape_estimation/model/convex_hull.hpp" #include #include @@ -28,6 +28,11 @@ #include #include +namespace autoware::shape_estimation +{ +namespace model +{ + bool ConvexHullShapeModel::estimate( const pcl::PointCloud & cluster, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) @@ -94,3 +99,6 @@ bool ConvexHullShapeModel::estimate( pose_output.orientation.w = 1; return true; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/model/cylinder.cpp b/perception/shape_estimation/lib/model/cylinder.cpp index 9b3f58854df9d..3f3139222a2bc 100644 --- a/perception/shape_estimation/lib/model/cylinder.cpp +++ b/perception/shape_estimation/lib/model/cylinder.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/model/cylinder.hpp" +#include "autoware/shape_estimation/model/cylinder.hpp" #include #include @@ -26,6 +26,11 @@ #include +namespace autoware::shape_estimation +{ +namespace model +{ + bool CylinderShapeModel::estimate( const pcl::PointCloud & cluster, autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) @@ -63,3 +68,6 @@ bool CylinderShapeModel::estimate( pose_output.orientation.w = 1; return true; } + +} // namespace model +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index 85bc3cd8bd9d9..34eed5e641368 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" -#include "shape_estimation/corrector/corrector.hpp" -#include "shape_estimation/filter/filter.hpp" -#include "shape_estimation/model/model.hpp" +#include "autoware/shape_estimation/corrector/corrector.hpp" +#include "autoware/shape_estimation/filter/filter.hpp" +#include "autoware/shape_estimation/model/model.hpp" #include #include +namespace autoware::shape_estimation +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; ShapeEstimator::ShapeEstimator(bool use_corrector, bool use_filter, bool use_boost_bbox_optimizer) @@ -75,15 +78,15 @@ bool ShapeEstimator::estimateOriginalShapeAndPose( autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output) { // estimate shape - std::unique_ptr model_ptr; + std::unique_ptr model_ptr; if ( label == Label::CAR || label == Label::TRUCK || label == Label::BUS || label == Label::TRAILER || label == Label::MOTORCYCLE || label == Label::BICYCLE) { - model_ptr.reset(new BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer_)); + model_ptr.reset(new model::BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer_)); } else if (label == Label::PEDESTRIAN) { - model_ptr.reset(new CylinderShapeModel()); + model_ptr.reset(new model::CylinderShapeModel()); } else { - model_ptr.reset(new ConvexHullShapeModel()); + model_ptr.reset(new model::ConvexHullShapeModel()); } return model_ptr->estimate(cluster, shape_output, pose_output); @@ -93,17 +96,17 @@ bool ShapeEstimator::applyFilter( const uint8_t label, const autoware_perception_msgs::msg::Shape & shape, const geometry_msgs::msg::Pose & pose) { - std::unique_ptr filter_ptr; + std::unique_ptr filter_ptr; if (label == Label::CAR) { - filter_ptr.reset(new CarFilter); + filter_ptr.reset(new filter::CarFilter); } else if (label == Label::BUS) { - filter_ptr.reset(new BusFilter); + filter_ptr.reset(new filter::BusFilter); } else if (label == Label::TRUCK) { - filter_ptr.reset(new TruckFilter); + filter_ptr.reset(new filter::TruckFilter); } else if (label == Label::TRAILER) { - filter_ptr.reset(new TrailerFilter); + filter_ptr.reset(new filter::TrailerFilter); } else { - filter_ptr.reset(new NoFilter); + filter_ptr.reset(new filter::NoFilter); } return filter_ptr->filter(shape, pose); @@ -114,23 +117,26 @@ bool ShapeEstimator::applyCorrector( const boost::optional & ref_shape_size_info, autoware_perception_msgs::msg::Shape & shape, geometry_msgs::msg::Pose & pose) { - std::unique_ptr corrector_ptr; + std::unique_ptr corrector_ptr; if (ref_shape_size_info && use_reference_yaw) { - corrector_ptr.reset(new ReferenceShapeBasedVehicleCorrector(ref_shape_size_info.get())); + corrector_ptr.reset( + new corrector::ReferenceShapeBasedVehicleCorrector(ref_shape_size_info.get())); } else if (label == Label::CAR) { - corrector_ptr.reset(new CarCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::CarCorrector(use_reference_yaw)); } else if (label == Label::BUS) { - corrector_ptr.reset(new BusCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::BusCorrector(use_reference_yaw)); } else if (label == Label::TRUCK) { - corrector_ptr.reset(new TruckCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::TruckCorrector(use_reference_yaw)); } else if (label == Label::TRAILER) { - corrector_ptr.reset(new TrailerCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::TrailerCorrector(use_reference_yaw)); } else if (label == Label::MOTORCYCLE || label == Label::BICYCLE) { - corrector_ptr.reset(new BicycleCorrector(use_reference_yaw)); + corrector_ptr.reset(new corrector::BicycleCorrector(use_reference_yaw)); } else { - corrector_ptr.reset(new NoCorrector); + corrector_ptr.reset(new corrector::NoCorrector); } return corrector_ptr->correct(shape, pose); } + +} // namespace autoware::shape_estimation diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/shape_estimation_node.cpp similarity index 93% rename from perception/shape_estimation/src/node.cpp rename to perception/shape_estimation/src/shape_estimation_node.cpp index 5719cd97b3ed5..4a373b15d580f 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/shape_estimation_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/shape_estimator.hpp" +#include "shape_estimation_node.hpp" -#include -#include +#include "autoware/shape_estimation/shape_estimator.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" #include #include @@ -32,6 +32,9 @@ #include #include +namespace autoware::shape_estimation +{ + using Label = autoware_perception_msgs::msg::ObjectClassification; ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_options) @@ -143,6 +146,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } +} // namespace autoware::shape_estimation #include -RCLCPP_COMPONENTS_REGISTER_NODE(ShapeEstimationNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::shape_estimation::ShapeEstimationNode) diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/shape_estimation_node.hpp similarity index 88% rename from perception/shape_estimation/src/node.hpp rename to perception/shape_estimation/src/shape_estimation_node.hpp index 807f12e39b025..d9f944346611a 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/shape_estimation_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NODE_HPP_ -#define NODE_HPP_ +#ifndef SHAPE_ESTIMATION_NODE_HPP_ +#define SHAPE_ESTIMATION_NODE_HPP_ -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include @@ -27,6 +27,9 @@ #include +namespace autoware::shape_estimation +{ + using autoware_perception_msgs::msg::DetectedObjects; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; class ShapeEstimationNode : public rclcpp::Node @@ -51,5 +54,6 @@ class ShapeEstimationNode : public rclcpp::Node public: explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options); }; +} // namespace autoware::shape_estimation -#endif // NODE_HPP_ +#endif // SHAPE_ESTIMATION_NODE_HPP_ diff --git a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp index 0c41e04e5ddd2..2e6dcc62d8699 100644 --- a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp +++ b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "shape_estimation/corrector/corrector.hpp" -#include "shape_estimation/filter/filter.hpp" -#include "shape_estimation/model/model.hpp" -#include "shape_estimation/shape_estimator.hpp" +#include "autoware/shape_estimation/corrector/corrector.hpp" +#include "autoware/shape_estimation/filter/filter.hpp" +#include "autoware/shape_estimation/model/model.hpp" +#include "autoware/shape_estimation/shape_estimator.hpp" #include #include @@ -70,7 +70,7 @@ pcl::PointCloud createLShapeCluster( TEST(BoundingBoxShapeModel, test_estimateShape) { // Generate BoundingBoxShapeModel - BoundingBoxShapeModel bbox_shape_model = BoundingBoxShapeModel(); + auto bbox_shape_model = autoware::shape_estimation::model::BoundingBoxShapeModel(); // Generate cluster const double length = 4.0; @@ -117,12 +117,12 @@ TEST(BoundingBoxShapeModel, test_estimateShape_rotated) pcl::PointCloud cluster = createLShapeCluster(length, width, height, yaw, offset_x, offset_y); - const auto ref_yaw_info = - ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + const auto ref_yaw_info = autoware::shape_estimation::ReferenceYawInfo{ + static_cast(yaw), static_cast(deg2rad(10.0))}; const bool use_boost_bbox_optimizer = true; // Generate BoundingBoxShapeModel - BoundingBoxShapeModel bbox_shape_model = - BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer); + auto bbox_shape_model = autoware::shape_estimation::model::BoundingBoxShapeModel( + ref_yaw_info, use_boost_bbox_optimizer); // Generate shape and pose output autoware_perception_msgs::msg::Shape shape_output; @@ -152,7 +152,7 @@ TEST(BoundingBoxShapeModel, test_estimateShape_rotated) TEST(CylinderShapeModel, test_estimateShape) { // Generate CylinderShapeModel - CylinderShapeModel cylinder_shape_model = CylinderShapeModel(); + auto cylinder_shape_model = autoware::shape_estimation::model::CylinderShapeModel(); // Generate cluster pcl::PointCloud cluster = createLShapeCluster(4.0, 2.0, 1.0, 0.0, 0.0, 0.0); @@ -169,7 +169,7 @@ TEST(CylinderShapeModel, test_estimateShape) TEST(ConvexHullShapeModel, test_estimateShape) { // Generate ConvexHullShapeModel - ConvexHullShapeModel convex_hull_shape_model = ConvexHullShapeModel(); + auto convex_hull_shape_model = autoware::shape_estimation::model::ConvexHullShapeModel(); // Generate cluster pcl::PointCloud cluster = createLShapeCluster(2.0, 1.0, 1.0, 0.0, 0.0, 0.0); @@ -200,14 +200,16 @@ TEST(ShapeEstimator, test_estimateShapeAndPose) const bool use_corrector = true; const bool use_filter = true; const bool use_boost_bbox_optimizer = true; - ShapeEstimator shape_estimator = - ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer); + auto shape_estimator = + autoware::shape_estimation::ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer); // Generate ref_yaw_info - boost::optional ref_yaw_info = boost::none; - boost::optional ref_shape_size_info = boost::none; + boost::optional ref_yaw_info = boost::none; + boost::optional ref_shape_size_info = + boost::none; - ref_yaw_info = ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + ref_yaw_info = autoware::shape_estimation::ReferenceYawInfo{ + static_cast(yaw), static_cast(deg2rad(10.0))}; const auto label = autoware_perception_msgs::msg::ObjectClassification::CAR; // Generate shape and pose output diff --git a/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 782012ff1eaa1..f69b31f70969e 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/simple_object_merger/README.md @@ -25,7 +25,7 @@ Merged objects will not be published until all topic data is received when initi - Post-processing -Because this package does not have matching processing, there are overlapping objects depending on the input objects. So output objects can be used only when post-processing is used. For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. +Because this package does not have matching processing, there are overlapping objects depending on the input objects. So output objects can be used only when post-processing is used. For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_radar_object_clustering) can be used as post-processing. ## Interface diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index a5498a845e62e..cac574aff8623 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -11,13 +11,69 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(OpenCV REQUIRED) -include(CheckLanguage) -check_language(CUDA) -if(CMAKE_CUDA_COMPILER) - enable_language(CUDA) + +option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) + +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if(CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if(CUDA_VERBOSE) + message("CUDA is available!") + message("CUDA Libs: ${CUDA_LIBRARIES}") + message("CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif() + # Note: cublas_device was depreciated in CUDA version 9.2 + # https://forums.developer.nvidia.com/t/where-can-i-find-libcublas-device-so-or-libcublas-device-a/67251/4 + # In LibTorch, CUDA_cublas_device_LIBRARY is used. + unset(CUDA_cublas_device_LIBRARY CACHE) + set(CUDA_AVAIL ON) +else() + message("CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif() + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER nvinfer) +find_library(NVONNXPARSER nvonnxparser) +if(NVINFER AND NVONNXPARSER) + if(CUDA_VERBOSE) + message("TensorRT is available!") + message("NVINFER: ${NVINFER}") + message("NVONNXPARSER: ${NVONNXPARSER}") + endif() + set(TRT_AVAIL ON) else() - message(WARNING "CUDA is not found. preprocess acceleration using CUDA will not be available.") + message("TensorRT is NOT Available") + set(TRT_AVAIL OFF) endif() + +# set flags for CUDNN availability +option(CUDNN_AVAIL "CUDNN available" OFF) +# try to find the CUDNN module +find_library(CUDNN_LIBRARY +NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} +PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} +PATH_SUFFIXES lib lib64 bin +DOC "CUDNN library." +) +if(CUDNN_LIBRARY) + if(CUDA_VERBOSE) + message(STATUS "CUDNN is available!") + message(STATUS "CUDNN_LIBRARY: ${CUDNN_LIBRARY}") + endif() + set(CUDNN_AVAIL ON) +else() + message("CUDNN is NOT Available") + set(CUDNN_AVAIL OFF) +endif() + find_package(OpenMP) if(OpenMP_FOUND) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") @@ -34,7 +90,7 @@ ament_target_dependencies(${PROJECT_NAME} OpenCV ) -if(CMAKE_CUDA_COMPILER) +if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # Officially, add_library supports .cu file compilation. # However, as of cmake 3.22.1, it seems to fail compilation because compiler flags for # C++ are directly passed to nvcc (they are originally space separated diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index 34155c3477003..25f762e2a4ec8 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -130,12 +130,12 @@ std::vector get_seg_colormap(const std::string & filen cmap.name = name; colormapString.erase(0, npos + 1); while (!colormapString.empty()) { - size_t npos = colormapString.find_first_of(','); - if (npos != std::string::npos) { - substr = colormapString.substr(0, npos); + size_t inner_npos = colormapString.find_first_of(','); + if (inner_npos != std::string::npos) { + substr = colormapString.substr(0, inner_npos); unsigned char c = (unsigned char)std::stoi(trim(substr)); cmap.color.push_back(c); - colormapString.erase(0, npos + 1); + colormapString.erase(0, inner_npos + 1); } else { unsigned char c = (unsigned char)std::stoi(trim(colormapString)); cmap.color.push_back(c); @@ -508,29 +508,16 @@ void TrtYoloX::preprocess(const std::vector & images) const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; scales_.clear(); - bool letterbox = true; - if (letterbox) { - for (const auto & image : images) { - cv::Mat dst_image; - const float scale = std::min(input_width / image.cols, input_height / image.rows); - scales_.emplace_back(scale); - const auto scale_size = cv::Size(image.cols * scale, image.rows * scale); - cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - const auto bottom = input_height - dst_image.rows; - const auto right = input_width - dst_image.cols; - copyMakeBorder( - dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); - dst_images.emplace_back(dst_image); - } - } else { - for (const auto & image : images) { - cv::Mat dst_image; - const float scale = -1.0; - scales_.emplace_back(scale); - const auto scale_size = cv::Size(input_width, input_height); - cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - dst_images.emplace_back(dst_image); - } + for (const auto & image : images) { + cv::Mat dst_image; + const float scale = std::min(input_width / image.cols, input_height / image.rows); + scales_.emplace_back(scale); + const auto scale_size = cv::Size(image.cols * scale, image.rows * scale); + cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); + const auto bottom = input_height - dst_image.rows; + const auto right = input_width - dst_image.cols; + copyMakeBorder(dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); + dst_images.emplace_back(dst_image); } const auto chw_images = cv::dnn::blobFromImages( dst_images, norm_factor_, cv::Size(), cv::Scalar(), false, false, CV_32F); @@ -650,34 +637,21 @@ void TrtYoloX::preprocessWithRoi( const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; scales_.clear(); - bool letterbox = true; int b = 0; - if (letterbox) { - for (const auto & image : images) { - cv::Mat dst_image; - cv::Mat cropped = image(rois[b]); - const float scale = std::min( - input_width / static_cast(rois[b].width), - input_height / static_cast(rois[b].height)); - scales_.emplace_back(scale); - const auto scale_size = cv::Size(rois[b].width * scale, rois[b].height * scale); - cv::resize(cropped, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - const auto bottom = input_height - dst_image.rows; - const auto right = input_width - dst_image.cols; - copyMakeBorder( - dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); - dst_images.emplace_back(dst_image); - b++; - } - } else { - for (const auto & image : images) { - cv::Mat dst_image; - const float scale = -1.0; - scales_.emplace_back(scale); - const auto scale_size = cv::Size(input_width, input_height); - cv::resize(image, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); - dst_images.emplace_back(dst_image); - } + for (const auto & image : images) { + cv::Mat dst_image; + cv::Mat cropped = image(rois[b]); + const float scale = std::min( + input_width / static_cast(rois[b].width), + input_height / static_cast(rois[b].height)); + scales_.emplace_back(scale); + const auto scale_size = cv::Size(rois[b].width * scale, rois[b].height * scale); + cv::resize(cropped, dst_image, scale_size, 0, 0, cv::INTER_CUBIC); + const auto bottom = input_height - dst_image.rows; + const auto right = input_width - dst_image.cols; + copyMakeBorder(dst_image, dst_image, 0, bottom, 0, right, cv::BORDER_CONSTANT, {114, 114, 114}); + dst_images.emplace_back(dst_image); + b++; } const auto chw_images = cv::dnn::blobFromImages( dst_images, norm_factor_, cv::Size(), cv::Scalar(), false, false, CV_32F); @@ -1231,8 +1205,8 @@ cv::Mat TrtYoloX::getMaskImageGpu(float * d_prob, nvinfer1::Dims dims, int out_w cv::Mat mask = cv::Mat::zeros(out_h, out_w, CV_8UC1); int index = b * out_w * out_h; argmax_gpu( - (unsigned char *)argmax_buf_d_.get() + index, d_prob, out_w, out_h, width, height, classes, 1, - *stream_); + reinterpret_cast(argmax_buf_d_.get()) + index, d_prob, out_w, out_h, width, + height, classes, 1, *stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( argmax_buf_h_.get(), argmax_buf_d_.get(), sizeof(unsigned char) * 1 * out_w * out_h, cudaMemcpyDeviceToHost, *stream_)); diff --git a/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp index 7be715b24bd5f..7d792c157d356 100644 --- a/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp +++ b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp @@ -328,12 +328,12 @@ void SSP::maximizeLinearAssignment( #ifndef NDEBUG // Check if the potentials are feasible potentials - for (int v = 0; v < n_nodes; ++v) { - for (auto it_incident_edge = adjacency_list.at(v).cbegin(); - it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + for (int w = 0; w < n_nodes; ++w) { + for (auto it_incident_edge = adjacency_list.at(w).cbegin(); + it_incident_edge != adjacency_list.at(w).cend(); ++it_incident_edge) { if (it_incident_edge->capacity > 0) { double reduced_cost = - it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + it_incident_edge->cost + potentials.at(w) - potentials.at(it_incident_edge->dst); assert(reduced_cost >= 0); } } diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp index ae19f51876490..e0ef193b8306b 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp @@ -58,20 +58,20 @@ class DecorativeTrackerMergerNode : public rclcpp::Node std::unordered_map> & data_association_map); void mainObjectsCallback( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & main_objects); void subObjectsCallback( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & msg); bool decorativeMerger( - const MEASUREMENT_STATE input_index, - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + const MEASUREMENT_STATE input_sensor, + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_objects_msg); autoware_perception_msgs::msg::TrackedObjects predictFutureState( const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg, const std_msgs::msg::Header & header); std::optional interpolateObjectState( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg1, - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg2, - const std_msgs::msg::Header & header); + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & former_msg, + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & latter_msg, + const std_msgs::msg::Header & output_header); TrackedObjects getTrackedObjects(const std_msgs::msg::Header & header); TrackerState createNewTracker( const MEASUREMENT_STATE input_index, rclcpp::Time current_time, diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 0202a26e46ef6..bc668c6af6c3c 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -129,10 +129,10 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( const uint8_t traffic_light_type) { std::vector occlusion_ratios; - size_t not_detected_roi = 0; if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) { occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { + size_t not_detected_roi = 0; tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg; selected_roi_msg.rois.reserve(in_roi_msg->rois.size()); for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) { diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 6515a515397da..40e50aabf3c17 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -211,7 +211,9 @@ uint32_t CloudOcclusionPredictor::predict( { const uint32_t horizontal_sample_num = 20; const uint32_t vertical_sample_num = 20; - static_assert(horizontal_sample_num > 1 && vertical_sample_num > 1); + static_assert(horizontal_sample_num > 1); + static_assert(vertical_sample_num > 1); + const float min_dist_from_occlusion_to_tl = 5.0f; pcl::PointCloud tl_sample_cloud; diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index 2f912e30b8246..452d0a64d5c9c 100644 --- a/planning/autoware_mission_planner/README.md +++ b/planning/autoware_mission_planner/README.md @@ -47,10 +47,11 @@ It distributes route requests and planning results according to current MRM oper ### Subscriptions -| Name | Type | Description | -| --------------------- | ----------------------------------- | ---------------------- | -| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | -| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| Name | Type | Description | +| ---------------------------- | ----------------------------------------- | ---------------------- | +| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | +| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| `input/operation_mode_state` | autoware_adapi_v1_msgs/OperationModeState | operation mode state | ### Publications @@ -170,6 +171,7 @@ To calculate `route_lanelets`, ### Rerouting Reroute here means changing the route while driving. Unlike route setting, it is required to keep a certain distance from vehicle to the point where the route is changed. +If the ego vehicle is not on autonomous driving state, the safety checking process will be skipped. ![rerouting_safety](./media/rerouting_safety.svg) diff --git a/planning/autoware_mission_planner/launch/mission_planner.launch.xml b/planning/autoware_mission_planner/launch/mission_planner.launch.xml index 655662c392213..8d77e417a6379 100644 --- a/planning/autoware_mission_planner/launch/mission_planner.launch.xml +++ b/planning/autoware_mission_planner/launch/mission_planner.launch.xml @@ -10,6 +10,7 @@ + diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp index 60c2a53b82123..19e28ee2abfc7 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -52,7 +52,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin void clearRoute() override; MarkerArray visualize(const LaneletRoute & route) const override; MarkerArray visualize_debug_footprint( - autoware::universe_utils::LinearRing2d goal_footprint_) const; + autoware::universe_utils::LinearRing2d goal_footprint) const; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; private: diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp index 0df01257f049e..2b1b943e002a8 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp @@ -55,6 +55,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); + sub_operation_mode_state_ = create_subscription( + "~/input/operation_mode_state", rclcpp::QoS(1), + std::bind(&MissionPlanner::on_operation_mode_state, this, _1)); sub_vector_map_ = create_subscription( "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); pub_marker_ = create_publisher("~/debug/route_marker", durable_qos); @@ -130,6 +133,11 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } +void MissionPlanner::on_operation_mode_state(const OperationModeState::ConstSharedPtr msg) +{ + operation_mode_state_ = msg; +} + void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg) { map_ptr_ = msg; @@ -222,10 +230,23 @@ void MissionPlanner::on_set_lanelet_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - const auto reroute_availability = sub_reroute_availability_.takeData(); - if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { + if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received."); + } + + const bool is_autonomous_driving = + operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS && + operation_mode_state_->is_autoware_control_enabled + : false; + + if (is_reroute && is_autonomous_driving) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (!reroute_availability || !reroute_availability->availability) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, + "Cannot reroute as the planner is not in lane following."); + } } change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); @@ -238,7 +259,7 @@ void MissionPlanner::on_set_lanelet_route( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - if (is_reroute && !check_reroute_safety(*current_route_, route)) { + if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) { cancel_route(); change_state(RouteState::SET); throw service_utils::ServiceException( @@ -271,10 +292,23 @@ void MissionPlanner::on_set_waypoint_route( throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - const auto reroute_availability = sub_reroute_availability_.takeData(); - if (is_reroute && (!reroute_availability || !reroute_availability->availability)) { + if (is_reroute && !operation_mode_state_) { throw service_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + ResponseCode::ERROR_PLANNER_UNREADY, "Operation mode state is not received."); + } + + const bool is_autonomous_driving = + operation_mode_state_ ? operation_mode_state_->mode == OperationModeState::AUTONOMOUS && + operation_mode_state_->is_autoware_control_enabled + : false; + + if (is_reroute && is_autonomous_driving) { + const auto reroute_availability = sub_reroute_availability_.takeData(); + if (!reroute_availability || !reroute_availability->availability) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, + "Cannot reroute as the planner is not in lane following."); + } } change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); @@ -287,7 +321,7 @@ void MissionPlanner::on_set_waypoint_route( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - if (is_reroute && !check_reroute_safety(*current_route_, route)) { + if (is_reroute && is_autonomous_driving && !check_reroute_safety(*current_route_, route)) { cancel_route(); change_state(RouteState::SET); throw service_utils::ServiceException( diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp index 1a04a91c14ba3..1181ef54273ae 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -45,6 +46,7 @@ namespace autoware::mission_planner { +using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; @@ -85,18 +87,21 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_operation_mode_state_; autoware::universe_utils::InterProcessPollingSubscriber sub_reroute_availability_{this, "~/input/reroute_availability"}; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Publisher::SharedPtr pub_marker_; Odometry::ConstSharedPtr odometry_; + OperationModeState::ConstSharedPtr operation_mode_state_; LaneletMapBin::ConstSharedPtr map_ptr_; RouteState state_; LaneletRoute::ConstSharedPtr current_route_; lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_operation_mode_state(const OperationModeState::ConstSharedPtr msg); void on_map(const LaneletMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index fc8a0a96ade42..f2a475c494aba 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -25,6 +25,8 @@ #include +#include + #include #include #include @@ -286,6 +288,9 @@ struct DebugData MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; std::vector detection_polygons; + std::optional stop_reason_diag{std::nullopt}; + std::optional slow_down_reason_diag{std::nullopt}; + std::optional cruise_reason_diag{std::nullopt}; }; struct EgoNearestParam diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index b96605f57b943..d4094ff08c3f0 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -192,10 +192,13 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // planner std::unique_ptr planner_ptr_{nullptr}; - // previous obstacles - std::vector prev_stop_obstacles_; - std::vector prev_cruise_obstacles_; - std::vector prev_slow_down_obstacles_; + // previous PredictedObject-based obstacles + std::vector prev_stop_object_obstacles_; + std::vector prev_cruise_object_obstacles_; + std::vector prev_slow_down_object_obstacles_; + + // PointCloud-based stop obstacle history + std::vector stop_pc_obstacle_history_; // behavior determination parameter struct BehaviorDeterminationParam @@ -303,7 +306,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_slow_down_planning_{false}; bool use_pointcloud_{false}; - std::vector prev_closest_stop_obstacles_{}; + std::vector prev_closest_stop_object_obstacles_{}; std::unique_ptr logger_configure_; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 3679c253e2bbb..099d80306bb4e 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -23,6 +23,8 @@ #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include + #include #include #include @@ -33,6 +35,8 @@ #include #include +using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; +using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; class PlannerInterface { public: @@ -52,6 +56,7 @@ class PlannerInterface node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); + diagnostics_pub_ = node.create_publisher("/diagnostics", 10); moving_object_speed_threshold = node.declare_parameter("slow_down.moving_object_speed_threshold"); @@ -90,6 +95,14 @@ class PlannerInterface const std::vector & slow_down_obstacles, std::optional & vel_limit); + DiagnosticStatus makeEmptyDiagnostic(const std::string & reason); + DiagnosticStatus makeDiagnostic( + const std::string & reason, const std::optional & planner_data = std::nullopt, + const std::optional & stop_pose = std::nullopt, + const std::optional & stop_obstacle = std::nullopt); + void publishDiagnostics(const rclcpp::Time & current_time); + void clearDiagnostics(); + void onParam(const std::vector & parameters) { updateCommonParam(parameters); @@ -135,6 +148,7 @@ class PlannerInterface rclcpp::Publisher::SharedPtr stop_reasons_pub_; rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; // Vehicle Parameters autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index c8f962c3ad711..210577d92ef93 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -557,7 +557,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto objects_ptr = objects_sub_.takeData(); const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr; const auto acc_ptr = acc_sub_.takeData(); - if (!ego_odom_ptr || (!objects_ptr && (!use_pointcloud_ || !pointcloud_ptr)) || !acc_ptr) { + const bool can_detect_obstacles = objects_ptr || pointcloud_ptr; + if (!ego_odom_ptr || !can_detect_obstacles || !acc_ptr) { return; } @@ -596,7 +597,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms concatenate(cruise_obstacles, cruise_object_obstacles); concatenate(slow_down_obstacles, slow_down_object_obstacles); } - if (pointcloud_ptr && !pointcloud_ptr->data.empty()) { + if (pointcloud_ptr) { const auto target_obstacles = convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); @@ -635,6 +636,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 8. Publish debug data published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); + planner_ptr_->publishDiagnostics(now()); publishDebugMarker(); publishDebugInfo(); @@ -821,7 +823,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( transform_stamped = std::nullopt; } - if (transform_stamped) { + if (!pointcloud.data.empty() && transform_stamped) { // 1. transform pointcloud PointCloud::Ptr pointcloud_ptr(new PointCloud); pcl::fromROSMsg(pointcloud, *pointcloud_ptr); @@ -1032,9 +1034,9 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( checkConsistency(objects.header.stamp, objects, stop_obstacles); // update previous obstacles - prev_stop_obstacles_ = stop_obstacles; - prev_cruise_obstacles_ = cruise_obstacles; - prev_slow_down_obstacles_ = slow_down_obstacles; + prev_stop_object_obstacles_ = stop_obstacles; + prev_cruise_object_obstacles_ = cruise_obstacles; + prev_slow_down_object_obstacles_ = slow_down_obstacles; const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( @@ -1051,6 +1053,8 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPointCloudObstacles( { stop_watch_.tic(__func__); + const auto & p = behavior_determination_param_; + // calculated decimated trajectory points and trajectory polygon const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); const auto decimated_traj_polys = @@ -1077,6 +1081,32 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPointCloudObstacles( } } + std::vector past_stop_obstacles; + for (auto itr = stop_pc_obstacle_history_.begin(); itr != stop_pc_obstacle_history_.end();) { + const double elapsed_time = (rclcpp::Time(odometry.header.stamp) - itr->stamp).seconds(); + if (elapsed_time >= p.stop_obstacle_hold_time_threshold) { + itr = stop_pc_obstacle_history_.erase(itr); + continue; + } + + const auto lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, itr->collision_point); + const auto min_lat_dist_to_traj_poly = + std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; + + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + auto stop_obstacle = *itr; + stop_obstacle.dist_to_collide_on_decimated_traj = autoware::motion_utils::calcSignedArcLength( + decimated_traj_points, 0, stop_obstacle.collision_point); + past_stop_obstacles.push_back(stop_obstacle); + } + + ++itr; + } + + concatenate(stop_pc_obstacle_history_, stop_obstacles); + concatenate(stop_obstacles, past_stop_obstacles); + const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", @@ -1307,7 +1337,7 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, // obstacle.uuid).has_value(); const bool is_prev_obstacle_cruise = - getObstacleFromUuid(prev_cruise_obstacles_, obstacle.uuid).has_value(); + getObstacleFromUuid(prev_cruise_object_obstacles_, obstacle.uuid).has_value(); if (is_prev_obstacle_cruise) { if (obstacle_tangent_vel < p.obstacle_velocity_threshold_from_cruise_to_stop) { @@ -1554,7 +1584,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); const bool is_prev_obstacle_slow_down = - getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); + getObstacleFromUuid(prev_slow_down_object_obstacles_, obstacle.uuid).has_value(); if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) { return std::nullopt; @@ -1693,7 +1723,7 @@ void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) { - for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_object_obstacles_) { const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&prev_closest_stop_obstacle](const PredictedObject & po) { @@ -1723,7 +1753,8 @@ void ObstacleCruisePlannerNode::checkConsistency( } } - prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); + prev_closest_stop_object_obstacles_ = + obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); } bool ObstacleCruisePlannerNode::isObstacleCrossing( diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index b3a0fc014c410..899cc66cb4331 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -199,6 +199,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( output.at(i).longitudinal_velocity_mps = 0.0; } prev_output_ = output; + debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); return output; } else if (opt_position.size() == 1) { RCLCPP_DEBUG( @@ -255,6 +256,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( // Insert Closest Stop Point autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); + debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); prev_output_ = output; return output; } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index d0736c6158a7a..a021e87e6199e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -323,6 +323,7 @@ std::vector PIDBasedPlanner::planCruise( // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); + debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 0413d4bbf3529..0b9a9eaa9d58c 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -25,6 +25,9 @@ #include #include +#include +#include + namespace { StopSpeedExceeded createStopSpeedExceededMsg( @@ -245,7 +248,6 @@ std::vector PlannerInterface::generateStopTrajectory( if (stop_obstacles.empty()) { stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time)); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time)); - // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); @@ -402,7 +404,9 @@ std::vector PlannerInterface::generateStopTrajectory( makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); - + // Store stop reason debug data + debug_data_ptr_->stop_reason_diag = + makeDiagnostic("stop", planner_data, stop_pose, *determined_stop_obstacle); // Publish if ego vehicle will over run against the stop point with a limit acceleration const bool will_over_run = determined_zero_vel_dist.value() > @@ -671,7 +675,11 @@ std::vector PlannerInterface::generateSlowDownTrajectory( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } + // Add debug data debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); + if (!debug_data_ptr_->slow_down_reason_diag.has_value()) { + debug_data_ptr_->slow_down_reason_diag = makeDiagnostic("slow_down", planner_data); + } // update prev_slow_down_output_ new_prev_slow_down_output.push_back(SlowDownOutput{ @@ -828,3 +836,90 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( return std::make_tuple( filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); } + +DiagnosticStatus PlannerInterface::makeEmptyDiagnostic(const std::string & reason) +{ + // Create status + DiagnosticStatus status; + status.level = status.OK; + status.name = "obstacle_cruise_planner_" + reason; + diagnostic_msgs::msg::KeyValue key_value; + { + // Decision + key_value.key = "decision"; + key_value.value = "none"; + status.values.push_back(key_value); + } + + return status; +} + +DiagnosticStatus PlannerInterface::makeDiagnostic( + const std::string & reason, const std::optional & planner_data, + const std::optional & stop_pose, + const std::optional & stop_obstacle) +{ + // Create status + DiagnosticStatus status; + status.level = status.OK; + status.name = "obstacle_cruise_planner_" + reason; + diagnostic_msgs::msg::KeyValue key_value; + { + // Decision + key_value.key = "decision"; + key_value.value = reason; + status.values.push_back(key_value); + } + + if (stop_pose.has_value() && planner_data.has_value()) { // Stop info + key_value.key = "stop_position"; + const auto & p = stop_pose.value().position; + key_value.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + status.values.push_back(key_value); + key_value.key = "stop_orientation"; + const auto & o = stop_pose.value().orientation; + key_value.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " + + std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; + status.values.push_back(key_value); + const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( + planner_data.value().traj_points, planner_data.value().ego_pose.position, + stop_pose.value().position); + key_value.key = "distance_to_stop_pose"; + key_value.value = std::to_string(dist_to_stop_pose); + status.values.push_back(key_value); + } + + if (stop_obstacle.has_value()) { + // Obstacle info + const auto & p = stop_obstacle.value().collision_point; + key_value.key = "collision_point"; + key_value.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + status.values.push_back(key_value); + } + + return status; +} + +void PlannerInterface::publishDiagnostics(const rclcpp::Time & current_time) +{ + // create array + DiagnosticArray diagnostics; + diagnostics.header.stamp = current_time; + diagnostics.header.frame_id = "map"; + const auto & d = debug_data_ptr_; + diagnostics.status = { + (d->stop_reason_diag) ? d->stop_reason_diag.value() : makeEmptyDiagnostic("stop"), + (d->slow_down_reason_diag) ? *(d->slow_down_reason_diag) : makeEmptyDiagnostic("slow_down"), + (d->cruise_reason_diag) ? d->cruise_reason_diag.value() : makeEmptyDiagnostic("cruise")}; + diagnostics_pub_->publish(diagnostics); + clearDiagnostics(); +} + +void PlannerInterface::clearDiagnostics() +{ + debug_data_ptr_->stop_reason_diag = std::nullopt; + debug_data_ptr_->slow_down_reason_diag = std::nullopt; + debug_data_ptr_->cruise_reason_diag = std::nullopt; +} diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index 7f2688ffaad95..399262f93e19d 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -17,7 +17,6 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -43,57 +42,6 @@ struct PlannerData double ego_vel{}; }; -struct TimeKeeper -{ - void init() - { - accumulated_msg = "\n"; - accumulated_time = 0.0; - } - - template - TimeKeeper & operator<<(const T & msg) - { - latest_stream << msg; - return *this; - } - - void endLine() - { - const auto msg = latest_stream.str(); - accumulated_msg += msg + "\n"; - latest_stream.str(""); - - if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_optimizer.time"), msg); - } - } - - void tic(const std::string & func_name) { stop_watch_.tic(func_name); } - - void toc(const std::string & func_name, const std::string & white_spaces) - { - const double elapsed_time = stop_watch_.toc(func_name); - *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; - accumulated_time = elapsed_time; - endLine(); - } - - std::string getLog() const { return accumulated_msg; } - - bool enable_calculation_time_info; - std::string accumulated_msg = "\n"; - std::stringstream latest_stream; - - double getAccumulatedTime() const { return accumulated_time; } - - double accumulated_time{0.0}; - - autoware::universe_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; -}; - struct DebugData { // settting diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 4303e5c7e05b4..3443ab663b642 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -19,6 +19,7 @@ #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" @@ -107,7 +108,7 @@ class MPTOptimizer rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr); + const std::shared_ptr time_keeper_); std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; @@ -222,7 +223,7 @@ class MPTOptimizer autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; rclcpp::Logger logger_; MPTParam mpt_param_; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 359c20f2a4d29..6f75c649e02b2 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -22,10 +22,11 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "rclcpp/rclcpp.hpp" #include +#include #include #include @@ -65,7 +66,7 @@ class PathOptimizer : public rclcpp::Node // argument variables autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; - mutable std::shared_ptr time_keeper_ptr_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; // flags for some functions bool enable_pub_debug_marker_; @@ -98,6 +99,8 @@ class PathOptimizer : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_markers_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index b8162f5ef2d32..1d23d7788dca1 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -18,6 +18,7 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -39,9 +40,9 @@ class StateEquationGenerator StateEquationGenerator() = default; StateEquationGenerator( const double wheel_base, const double max_steer_rad, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), - time_keeper_ptr_(time_keeper_ptr) + time_keeper_(time_keeper) { } @@ -56,7 +57,7 @@ class StateEquationGenerator private: std::unique_ptr vehicle_model_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index b5fd266006a48..45943d7deec09 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -395,13 +395,13 @@ MPTOptimizer::MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : enable_debug_info_(enable_debug_info), ego_nearest_param_(ego_nearest_param), vehicle_info_(vehicle_info), traj_param_(traj_param), debug_data_ptr_(debug_data_ptr), - time_keeper_ptr_(time_keeper_ptr), + time_keeper_(time_keeper), logger_(node->get_logger().get_child("mpt_optimizer")) { // initialize mpt param @@ -411,7 +411,7 @@ MPTOptimizer::MPTOptimizer( // state equation generator state_equation_generator_ = - StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_ptr_); + StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_); // osqp solver osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); @@ -470,7 +470,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -520,8 +520,6 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData // 8. publish trajectories for debug publishDebugTrajectories(p.header, ref_points, *mpt_traj_points); - time_keeper_ptr_->toc(__func__, " "); - debug_data_ptr_->ref_points = ref_points; prev_ref_points_ptr_ = std::make_shared>(ref_points); prev_optimized_traj_points_ptr_ = @@ -541,7 +539,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; @@ -549,14 +547,14 @@ std::vector MPTOptimizer::calcReferencePoints( const double backward_traj_length = traj_param_.output_backward_traj_length; // 1. resample and convert smoothed points type from trajectory points to reference points - time_keeper_ptr_->tic("resampleReferencePoints"); + time_keeper_->start_track("resampleReferencePoints"); auto ref_points = [&]() { const auto resampled_smoothed_points = trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( smoothed_points, mpt_param_.delta_arc_length); return trajectory_utils::convertToReferencePoints(resampled_smoothed_points); }(); - time_keeper_ptr_->toc("resampleReferencePoints", " "); + time_keeper_->end_track("resampleReferencePoints"); // 2. crop forward and backward with margin, and calculate spline interpolation // NOTE: Margin is added to calculate orientation, curvature, etc precisely. @@ -611,8 +609,6 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points.resize(mpt_param_.num_points); } - time_keeper_ptr_->toc(__func__, " "); - return ref_points; } @@ -639,7 +635,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!prev_ref_points_ptr_) { // no fixed point @@ -681,8 +677,6 @@ void MPTOptimizer::updateFixedPoint(std::vector & ref_points) co ref_points.front().curvature = front_point.curvature; ref_points.front().fixed_kinematic_state = front_point.optimized_kinematic_state; } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points) const @@ -697,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -786,8 +780,6 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c } } } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateBounds( @@ -796,7 +788,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -844,8 +836,6 @@ void MPTOptimizer::updateBounds( } } */ - - time_keeper_ptr_->toc(__func__, " "); return; } @@ -1104,7 +1094,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1160,8 +1150,6 @@ void MPTOptimizer::updateVehicleBounds( ref_points.at(p_idx).pose_on_constraints.push_back(vehicle_bounds_pose); } } - - time_keeper_ptr_->toc(__func__, " "); } // cost function: J = x' Q x + u' R u @@ -1169,7 +1157,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1228,7 +1216,6 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); - time_keeper_ptr_->toc(__func__, " "); return ValueMatrix{Q_sparse_mat, R_sparse_mat}; } @@ -1236,7 +1223,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1286,7 +1273,6 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( obj_matrix.hessian = H; obj_matrix.gradient = g; - time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } @@ -1297,7 +1283,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1451,7 +1437,6 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows_end += N_u; } - time_keeper_ptr_->toc(__func__, " "); return ConstraintMatrix{A, lb, ub}; } @@ -1477,7 +1462,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1510,7 +1495,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const auto lower_bound = toStdVector(updated_const_mat.lower_bound); // initialize or update solver according to warm start - time_keeper_ptr_->tic("initOsqp"); + time_keeper_->start_track("initOsqp"); const autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); @@ -1530,13 +1515,12 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( } prev_mat_n_ = H.rows(); prev_mat_m_ = A.rows(); - - time_keeper_ptr_->toc("initOsqp", " "); + time_keeper_->end_track("initOsqp"); // solve qp - time_keeper_ptr_->tic("solveOsqp"); + time_keeper_->start_track("solveOsqp"); const auto result = osqp_solver_ptr_->optimize(); - time_keeper_ptr_->toc("solveOsqp", " "); + time_keeper_->end_track("solveOsqp"); // check solution status const int solution_status = std::get<3>(result); @@ -1563,8 +1547,6 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const Eigen::VectorXd optimized_variables = Eigen::Map(&optimization_result[0], N_v); - time_keeper_ptr_->toc(__func__, " "); - if (u0) { // manual warm start return static_cast(optimized_variables + *u0); } @@ -1647,7 +1629,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1700,7 +1682,6 @@ std::optional> MPTOptimizer::calcMPTPoints( traj_points.push_back(traj_point); } - time_keeper_ptr_->toc(__func__, " "); return traj_points; } @@ -1708,7 +1689,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( @@ -1723,8 +1704,6 @@ void MPTOptimizer::publishDebugTrajectories( // mpt points const auto mpt_traj = autoware::motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); - - time_keeper_ptr_->toc(__func__, " "); } std::vector MPTOptimizer::extractFixedPoints( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 3899867a9dcce..7c73d1ad1fee3 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -23,6 +23,7 @@ #include "rclcpp/time.hpp" #include +#include #include namespace autoware::path_optimizer @@ -38,7 +39,8 @@ std::vector concatVectors(const std::vector & prev_vector, const std::vect return concatenated_vector; } -StringStamped createStringStamped(const rclcpp::Time & now, const std::string & data) +[[maybe_unused]] StringStamped createStringStamped( + const rclcpp::Time & now, const std::string & data) { StringStamped msg; msg.stamp = now; @@ -46,7 +48,7 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } -Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +[[maybe_unused]] Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) { Float64Stamped msg; msg.stamp = now; @@ -85,8 +87,7 @@ std::vector calcSegmentLengthVector(const std::vector & PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), - debug_data_ptr_(std::make_shared()), - time_keeper_ptr_(std::make_shared()) + debug_data_ptr_(std::make_shared()) { // interface publisher traj_pub_ = create_publisher("~/output/path", 1); @@ -102,6 +103,9 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); debug_calculation_time_float_pub_ = create_publisher("~/debug/processing_time_ms", 1); + debug_processing_time_detail_pub_ = + create_publisher( + "~/debug/processing_time_detail_ms", 1); { // parameters // parameter for option @@ -120,8 +124,6 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) // parameter for debug info enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); - time_keeper_ptr_->enable_calculation_time_info = - declare_parameter("option.debug.enable_calculation_time_info"); vehicle_stop_margin_outside_drivable_area_ = declare_parameter("common.vehicle_stop_margin_outside_drivable_area"); @@ -133,11 +135,14 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) traj_param_ = TrajectoryParam(this); } + time_keeper_ = + std::make_shared(debug_processing_time_detail_pub_); + // create core algorithm pointers with parameter declaration replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); mpt_optimizer_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, vehicle_info_, traj_param_, debug_data_ptr_, - time_keeper_ptr_); + time_keeper_); // reset planners // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been @@ -177,9 +182,6 @@ rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( // parameters for debug info updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); - updateParam( - parameters, "option.debug.enable_calculation_time_info", - time_keeper_ptr_->enable_calculation_time_info); updateParam( parameters, "common.vehicle_stop_margin_outside_drivable_area", @@ -220,8 +222,7 @@ void PathOptimizer::resetPreviousData() void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { - time_keeper_ptr_->init(); - time_keeper_ptr_->tic(__func__); + time_keeper_->start_track(__func__); // check if input path is valid if (!checkInputPath(*path_ptr, *get_clock())) { @@ -267,21 +268,21 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) // 5. publish debug data publishDebugData(planner_data.header); - time_keeper_ptr_->toc(__func__, ""); - *time_keeper_ptr_ << "========================================"; - time_keeper_ptr_->endLine(); - // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time + /* const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); debug_calculation_time_str_pub_->publish(calculation_time_msg); debug_calculation_time_float_pub_->publish( createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); + */ const auto output_traj_msg = autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); + + time_keeper_->end_track(__func__); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const @@ -319,7 +320,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & input_traj_points = planner_data.traj_points; @@ -339,13 +340,12 @@ std::vector PathOptimizer::generateOptimizedTrajectory( // 4. publish debug marker publishDebugMarkerOfOptimization(optimized_traj_points); - time_keeper_ptr_->toc(__func__, " "); return optimized_traj_points; } std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -372,7 +372,6 @@ std::vector PathOptimizer::optimizeTrajectory(const PlannerData // with model predictive trajectory const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data); - time_keeper_ptr_->toc(__func__, " "); return mpt_traj; } @@ -391,7 +390,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -486,14 +485,12 @@ void PathOptimizer::applyInputVelocity( trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); } } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (optimized_traj_points.empty()) { return; @@ -553,13 +550,11 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( } else { debug_data_ptr_->stop_pose_by_drivable_area = std::nullopt; } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); @@ -569,36 +564,33 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos } virtual_wall_pub_->publish(virtual_wall_marker); - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!enable_pub_debug_marker_) { return; } - time_keeper_ptr_->tic(__func__); - // debug marker - time_keeper_ptr_->tic("getDebugMarker"); + time_keeper_->start_track("getDebugMarker"); const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, enable_pub_extra_debug_marker_); - time_keeper_ptr_->toc("getDebugMarker", " "); + time_keeper_->end_track("getDebugMarker"); - time_keeper_ptr_->tic("publishDebugMarker"); + time_keeper_->start_track("publishDebugMarker"); debug_markers_pub_->publish(debug_marker); - time_keeper_ptr_->toc("publishDebugMarker", " "); - - time_keeper_ptr_->toc(__func__, " "); + time_keeper_->end_track("publishDebugMarker"); } std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -654,20 +646,17 @@ std::vector PathOptimizer::extendTrajectory( // debug_data_ptr_->extended_traj_points = // extended_traj_points ? *extended_traj_points : std::vector(); - time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } void PathOptimizer::publishDebugData(const Header & header) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // publish trajectories const auto debug_extended_traj = autoware::motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); - - time_keeper_ptr_->toc(__func__, " "); } } // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 5aa2ab4ffbfa8..74033c5834db2 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -23,7 +23,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); @@ -60,7 +60,6 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( W.segment(i * D_x, D_x) = Wd; } - time_keeper_ptr_->toc(__func__, " "); return Matrix{A, B, W}; } diff --git a/planning/autoware_planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp b/planning/autoware_planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp index 645d6c92e3698..4b5c495cc85b6 100644 --- a/planning/autoware_planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp +++ b/planning/autoware_planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp @@ -59,11 +59,6 @@ void InvalidTrajectoryPublisherNode::onTimer() traj_pub_->publish(output); RCLCPP_INFO(this->get_logger(), "invalid trajectory is published."); - - bool EXIT_AFTER_PUBLISH = false; - if (EXIT_AFTER_PUBLISH) { - exit(0); - } } void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 0540fb1758802..9811f982f8e1b 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -15,6 +15,7 @@ #include "autoware/route_handler/route_handler.hpp" #include +#include #include #include #include @@ -186,6 +187,16 @@ void RouteHandler::setMap(const LaneletMapBin & map_msg) lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( map_msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + const auto map_major_version_opt = + lanelet::io_handlers::parseMajorVersion(map_msg.version_map_format); + if (!map_major_version_opt) { + RCLCPP_WARN( + logger_, "setMap() for invalid version map: %s", map_msg.version_map_format.c_str()); + } else if (map_major_version_opt.value() > static_cast(lanelet::autoware::version)) { + RCLCPP_WARN( + logger_, "setMap() for a map(version %s) newer than lanelet2_extension support version(%d)", + map_msg.version_map_format.c_str(), static_cast(lanelet::autoware::version)); + } const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle); diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 0dd18615be5ef..26918cce24549 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -23,6 +23,7 @@ #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/ros/self_pose_listener.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" @@ -260,6 +261,8 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_closest_jerk_; rclcpp::Publisher::SharedPtr debug_calculation_time_; rclcpp::Publisher::SharedPtr debug_closest_max_velocity_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_; // For Jerk Filtered Algorithm Debug rclcpp::Publisher::SharedPtr pub_forward_filtered_trajectory_; @@ -275,6 +278,8 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; + + mutable std::shared_ptr time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 42d5a520c9e44..bbc3828bb1b15 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "rclcpp/rclcpp.hpp" @@ -24,6 +25,7 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include #include #include #include @@ -65,7 +67,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase } backward; }; - explicit AnalyticalJerkConstrainedSmoother(rclcpp::Node & node); + explicit AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper = + std::make_shared()); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index 14d0d8ab7ff84..06a6f2da7f30a 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -40,7 +42,8 @@ class JerkFilteredSmoother : public SmootherBase double jerk_filter_ds; }; - explicit JerkFilteredSmoother(rclcpp::Node & node); + explicit JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 1e2918d8e313b..a2a07ec6909aa 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -38,7 +40,8 @@ class L2PseudoJerkSmoother : public SmootherBase double over_a_weight; }; - explicit L2PseudoJerkSmoother(rclcpp::Node & node); + explicit L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index b88cd26eb4467..7c46a53431608 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -38,7 +40,8 @@ class LinfPseudoJerkSmoother : public SmootherBase double over_a_weight; }; - explicit LinfPseudoJerkSmoother(rclcpp::Node & node); + explicit LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index be7baf694d361..6671eaa3eabe7 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -15,12 +15,14 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include +#include #include namespace autoware::velocity_smoother @@ -54,7 +56,8 @@ class SmootherBase resampling::ResampleParam resample_param; }; - explicit SmootherBase(rclcpp::Node & node); + explicit SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper); virtual ~SmootherBase() = default; virtual bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, @@ -85,6 +88,7 @@ class SmootherBase protected: BaseParam base_param_; + mutable std::shared_ptr time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 5c66f5b906bba..ca7526e9adf3b 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -46,6 +46,13 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti initCommonParam(); over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); + // create time_keeper and its publisher + // NOTE: This has to be called before setupSmoother to pass the time_keeper to the smoother. + debug_processing_time_detail_ = create_publisher( + "~/debug/processing_time_detail_ms", 1); + time_keeper_ = + std::make_shared(debug_processing_time_detail_); + // create smoother setupSmoother(wheelbase_); @@ -99,7 +106,7 @@ void VelocitySmootherNode::setupSmoother(const double wheelbase) { switch (node_param_.algorithm_type) { case AlgorithmType::JERK_FILTERED: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); // Set Publisher for jerk filtered algorithm pub_forward_filtered_trajectory_ = @@ -113,15 +120,15 @@ void VelocitySmootherNode::setupSmoother(const double wheelbase) break; } case AlgorithmType::L2: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } case AlgorithmType::LINF: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } case AlgorithmType::ANALYTICAL: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } default: @@ -309,6 +316,8 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory void VelocitySmootherNode::calcExternalVelocityLimit() { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!external_velocity_limit_ptr_) { return; } @@ -414,6 +423,8 @@ bool VelocitySmootherNode::checkData() const void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); @@ -422,7 +433,7 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr // receive data current_odometry_ptr_ = sub_current_odometry_.takeData(); current_acceleration_ptr_ = sub_current_acceleration_.takeData(); - external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData(); + external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeNewData(); const auto operation_mode_ptr = sub_operation_mode_.takeData(); if (operation_mode_ptr) { operation_mode_ = *operation_mode_ptr; @@ -455,9 +466,6 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr calcExternalVelocityLimit(); updateDataForExternalVelocityLimit(); - // ignore current external velocity limit next time - external_velocity_limit_ptr_ = nullptr; - // For negative velocity handling, multiple -1 to velocity if it is for reverse. // NOTE: this process must be in the beginning of the process is_reverse_ = isReverse(input_points); @@ -505,6 +513,8 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr void VelocitySmootherNode::updateDataForExternalVelocityLimit() { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (prev_output_.empty()) { return; } @@ -522,6 +532,8 @@ void VelocitySmootherNode::updateDataForExternalVelocityLimit() TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( const TrajectoryPoints & traj_input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + TrajectoryPoints output{}; // velocity is optimized by qp solver // Extract trajectory around self-position with desired forward-backward length @@ -569,6 +581,8 @@ bool VelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.empty()) { return false; // cannot apply smoothing } @@ -675,6 +689,8 @@ bool VelocitySmootherNode::smoothVelocity( void VelocitySmootherNode::insertBehindVelocity( const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const bool keep_closest_vel_for_behind = (type == InitializeType::EGO_VELOCITY || type == InitializeType::LARGE_DEVIATION_REPLAN || type == InitializeType::ENGAGING); @@ -737,6 +753,8 @@ void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajecto std::pair VelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x; const double target_vel = std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps); @@ -820,6 +838,8 @@ std::pair VelocitySmootherNode::ca void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input); if (!stop_idx) { return; @@ -866,6 +886,8 @@ void VelocitySmootherNode::overwriteStopPoint( void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (traj.size() < 1) { return; } @@ -905,6 +927,8 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { return; // no stop point. diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 7f263fdea5e36..3a6bc736c96ab 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -66,8 +66,9 @@ bool applyMaxVelocity( namespace autoware::velocity_smoother { -AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) -: SmootherBase(node) +AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.resample.ds_resample = node.declare_parameter("resample.ds_resample"); @@ -373,14 +374,14 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt for (size_t i = 0; i < output.size(); ++i) { for (const auto & lat_acc_filtered_range : latacc_filtered_ranges) { - const size_t start_index = std::get<0>(lat_acc_filtered_range); - const size_t end_index = std::get<1>(lat_acc_filtered_range); - const double min_latacc_velocity = std::get<2>(lat_acc_filtered_range); + const size_t filtered_start_index = std::get<0>(lat_acc_filtered_range); + const size_t filtered_end_index = std::get<1>(lat_acc_filtered_range); + const double filtered_min_latacc_velocity = std::get<2>(lat_acc_filtered_range); if ( - start_index <= i && i <= end_index && + filtered_start_index <= i && i <= filtered_end_index && smoother_param_.latacc.enable_constant_velocity_while_turning) { - output.at(i).longitudinal_velocity_mps = min_latacc_velocity; + output.at(i).longitudinal_velocity_mps = filtered_min_latacc_velocity; break; } } @@ -414,15 +415,15 @@ bool AnalyticalJerkConstrainedSmoother::searchDecelTargetIndices( } if (!tmp_indices.empty()) { - for (unsigned int i = 0; i < tmp_indices.size() - 1; ++i) { + for (unsigned int j = 0; j < tmp_indices.size() - 1; ++j) { const size_t index_err = 10; if ( - (tmp_indices.at(i + 1).first - tmp_indices.at(i).first < index_err) && - (tmp_indices.at(i + 1).second < tmp_indices.at(i).second)) { + (tmp_indices.at(j + 1).first - tmp_indices.at(j).first < index_err) && + (tmp_indices.at(j + 1).second < tmp_indices.at(j).second)) { continue; } - decel_target_indices.emplace_back(tmp_indices.at(i).first, tmp_indices.at(i).second); + decel_target_indices.emplace_back(tmp_indices.at(j).first, tmp_indices.at(j).second); } } if (!tmp_indices.empty()) { diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index cab8e49a3b45d..d458c688d060c 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -29,7 +29,9 @@ namespace autoware::velocity_smoother { -JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) +JerkFilteredSmoother::JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.jerk_weight = node.declare_parameter("jerk_weight"); @@ -59,6 +61,8 @@ bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, std::vector & debug_trajectories) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + output = input; if (input.empty()) { @@ -102,6 +106,8 @@ bool JerkFilteredSmoother::apply( const auto initial_traj_pose = filtered.front().pose; const auto resample = [&](const auto & trajectory) { + autoware::universe_utils::ScopedTimeTrack st("resample", *time_keeper_); + return resampling::resampleTrajectory( trajectory, v0, initial_traj_pose, std::numeric_limits::max(), std::numeric_limits::max(), base_param_.resample_param); @@ -152,6 +158,7 @@ bool JerkFilteredSmoother::apply( v_max_arr.at(i) = opt_resampled_trajectory.at(i).longitudinal_velocity_mps; } + time_keeper_->start_track("initOptimization"); /* * x = [ * b[0], b[1], ..., b[N], : 0~N @@ -290,9 +297,12 @@ bool JerkFilteredSmoother::apply( lower_bound[constr_idx] = a0; ++constr_idx; } + time_keeper_->end_track("initOptimization"); // execute optimization + time_keeper_->start_track("optimize"); const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + time_keeper_->end_track("optimize"); const std::vector optval = std::get<0>(result); const int status_val = std::get<3>(result); if (status_val != 1) { @@ -356,6 +366,8 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( const double v0, const double a0, const double a_max, const double a_start, const double j_max, const TrajectoryPoints & input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + auto applyLimits = [&input, &a_start](double & v, double & a, size_t i) { double v_lim = input.at(i).longitudinal_velocity_mps; static constexpr double ep = 1.0e-5; @@ -408,6 +420,8 @@ TrajectoryPoints JerkFilteredSmoother::backwardJerkFilter( const double v0, const double a0, const double a_min, const double a_stop, const double j_min, const TrajectoryPoints & input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + auto input_rev = input; std::reverse(input_rev.begin(), input_rev.end()); auto filtered = forwardJerkFilter( @@ -423,6 +437,8 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + TrajectoryPoints merged; merged = forward_filtered; @@ -475,6 +491,8 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, const double nearest_yaw_threshold) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + return resampling::resampleTrajectory( input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, smoother_param_.jerk_filter_ds); diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index b489c994b0495..f379b217187c9 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -25,7 +25,9 @@ namespace autoware::velocity_smoother { -L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) +L2PseudoJerkSmoother::L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index cb8451dba8302..2ab3d6dd80dfc 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -25,7 +25,9 @@ namespace autoware::velocity_smoother { -LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) +LinfPseudoJerkSmoother::LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 700cf45b7eb9d..46faf10fe4a62 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -60,7 +60,9 @@ TrajectoryPoints applyPreProcess( } } // namespace -SmootherBase::SmootherBase(rclcpp::Node & node) +SmootherBase::SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: time_keeper_(time_keeper) { auto & p = base_param_; p.max_accel = node.declare_parameter("normal.max_acc"); @@ -130,6 +132,8 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, const bool use_resampling, const double input_points_interval) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.size() < 3) { return input; // cannot calculate lateral acc. do nothing. } @@ -198,6 +202,8 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( const TrajectoryPoints & input, const bool use_resampling, const double input_points_interval) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.size() < 3) { return input; // cannot calculate the desired velocity. do nothing. } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 02b90186d9b2f..8096d2944ee2b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -137,9 +137,11 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); - p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 856681d538db0..1a2e4439d4b7d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -113,10 +113,8 @@ void AvoidanceByLaneChange::updateSpecialData() : Direction::RIGHT; } - utils::static_obstacle_avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, p); - utils::static_obstacle_avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + utils::static_obstacle_avoidance::compensateLostTargetObjects( + registered_objects_, avoidance_data_, clock_.now(), planner_data_, p); std::sort( avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), @@ -136,7 +134,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( const auto resample_interval = avoidance_parameters_->resample_interval_for_planning; data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); - data.current_lanelets = getCurrentLanes(); + data.current_lanelets = get_current_lanes(); fillAvoidanceTargetObjects(data, debug); @@ -151,8 +149,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( *planner_data_->dynamic_object, data.current_lanelets, - [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); // Assume that the maximum allocation for data.other object is the sum of @@ -276,7 +274,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const { - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = get_current_lanes(); if (current_lanes.empty()) { RCLCPP_DEBUG(logger_, "no empty lanes"); return std::numeric_limits::infinity(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 6ea15a9309c3c..8f19613b50e6d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -206,20 +205,20 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left{false}; bool should_be_avoided{false}; - std::vector ref_path_points_for_obj_poly; + std::vector ref_points_for_obj_poly; LatFeasiblePaths ego_lat_feasible_paths; // add additional information (not update to the latest data) void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, const bool arg_is_collision_left, const bool arg_should_be_avoided, - const std::vector & arg_ref_path_points_for_obj_poly) + const std::vector & arg_ref_points_for_obj_poly) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; - ref_path_points_for_obj_poly = arg_ref_path_points_for_obj_poly; + ref_points_for_obj_poly = arg_ref_points_for_obj_poly; } }; @@ -317,12 +316,12 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, const bool should_be_avoided, - const std::vector & ref_path_points_for_obj_poly) + const std::vector & ref_points_for_obj_poly) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, - ref_path_points_for_obj_poly); + ref_points_for_obj_poly); } } @@ -409,24 +408,25 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::optional> calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( - const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const std::vector & ego_points, + const geometry_msgs::msg::Pose & obj_pose, const size_t obj_seg_idx, const autoware_perception_msgs::msg::Shape & obj_shape) const; double calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_perception_msgs::msg::Shape & obj_shape, const bool is_object_same_direction) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & ref_path_points_for_obj_poly, + const std::vector & ref_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape, const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoidRegulatedObject( - const std::vector & ref_path_points_for_obj_poly, + const std::vector & ref_points_for_obj_poly, const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, const bool is_collision_left, const double obj_normal_vel, const std::optional & prev_object) const; std::optional calcMinMaxLateralOffsetToAvoidUnregulatedObject( - const std::vector & ref_path_points_for_obj_poly, + const std::vector & ref_points_for_obj_poly, const std::optional & prev_object, const DynamicAvoidanceObject & object) const; std::pair getAdjacentLanes( @@ -454,10 +454,6 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::shared_ptr parameters_; TargetObjectsManager target_objects_manager_; - - mutable autoware::universe_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index 7fa9ed012b2b9..4d8cdef20a67a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -113,11 +113,11 @@ std::optional getObjectFromUuid(const std::vector & objects, const std::st } std::pair projectObstacleVelocityToTrajectory( - const std::vector & path_points, const PredictedObject & object) + const std::vector & path_points, const PredictedObject & object, + const size_t obj_idx) { const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; const double obj_yaw = tf2::getYaw(obj_pose.orientation); - const size_t obj_idx = autoware::motion_utils::findNearestIndex(path_points, obj_pose.position); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); const Eigen::Rotation2Dd R_ego_to_obstacle(obj_yaw - path_yaw); @@ -170,12 +170,9 @@ double calcObstacleWidth(const autoware_perception_msgs::msg::Shape & shape) double calcDiffAngleAgainstPath( const std::vector & path_points, - const geometry_msgs::msg::Pose & target_pose) + const geometry_msgs::msg::Pose & target_pose, const size_t target_idx) { - const size_t nearest_idx = - autoware::motion_utils::findNearestIndex(path_points, target_pose.position); - const double traj_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); - + const double traj_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); const double target_yaw = tf2::getYaw(target_pose.orientation); const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); @@ -202,32 +199,30 @@ double calcDiffAngleAgainstPath( } double calcDistanceToPath( - const std::vector & path_points, - const geometry_msgs::msg::Point & target_pos) + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_idx) { - const size_t target_idx = autoware::motion_utils::findNearestIndex(path_points, target_pos); - if (target_idx == 0 || target_idx == path_points.size() - 1) { - const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); - const double angle_to_target_pos = autoware::universe_utils::calcAzimuthAngle( - path_points.at(target_idx).point.pose.position, target_pos); + if (target_idx == 0 || target_idx == points.size() - 1) { + const double target_yaw = tf2::getYaw(points.at(target_idx).orientation); + const double angle_to_target_pos = + autoware::universe_utils::calcAzimuthAngle(points.at(target_idx).position, target_pos); const double diff_yaw = autoware::universe_utils::normalizeRadian(angle_to_target_pos - target_yaw); if ( (target_idx == 0 && (diff_yaw < -M_PI_2 || M_PI_2 < diff_yaw)) || - (target_idx == path_points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { - return autoware::universe_utils::calcDistance2d(path_points.at(target_idx), target_pos); + (target_idx == points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { + return autoware::universe_utils::calcDistance2d(points.at(target_idx), target_pos); } } - return std::abs(autoware::motion_utils::calcLateralOffset(path_points, target_pos)); + return std::abs(autoware::motion_utils::calcLateralOffset(points, target_pos)); } bool isLeft( const std::vector & path_points, - const geometry_msgs::msg::Point & target_pos) + const geometry_msgs::msg::Point & target_pos, const size_t target_idx) { - const size_t target_idx = autoware::motion_utils::findNearestIndex(path_points, target_pos); const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); const double angle_to_target_pos = autoware::universe_utils::calcAzimuthAngle( path_points.at(target_idx).point.pose.position, target_pos); @@ -303,6 +298,32 @@ std::vector convertToPoints( } return points; } + +// NOTE: Giving PathPointWithLaneId to autoware_motion_utils functions +// cost a lot. Instead, using Pose is much faster (around x10). +std::vector toGeometryPoints( + const std::vector & path_points) +{ + std::vector geom_points; + for (const auto & path_point : path_points) { + geom_points.push_back(path_point.point.pose); + } + return geom_points; +} + +size_t getNearestIndexFromSegmentIndex( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & target_pos) +{ + const double first_dist = + autoware::universe_utils::calcDistance2d(points.at(seg_idx), target_pos); + const double second_dist = + autoware::universe_utils::calcDistance2d(points.at(seg_idx + 1), target_pos); + if (first_dist < second_dist) { + return seg_idx; + } + return seg_idx + 1; +} } // namespace DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( @@ -351,7 +372,7 @@ bool DynamicObstacleAvoidanceModule::isExecutionReady() const void DynamicObstacleAvoidanceModule::updateData() { - // stop_watch_.tic(std::string(__func__)); + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); info_marker_.markers.clear(); debug_marker_.markers.clear(); @@ -377,11 +398,6 @@ void DynamicObstacleAvoidanceModule::updateData() target_objects_.push_back(target_object_candidate); } } - - // const double calculation_time = stop_watch_.toc(std::string(__func__)); - // RCLCPP_INFO_STREAM_EXPRESSION( - // getLogger(), parameters_->enable_debug_info, __func__ << ":=" << calculation_time << " - // [ms]"); } bool DynamicObstacleAvoidanceModule::canTransitSuccessState() @@ -391,8 +407,6 @@ bool DynamicObstacleAvoidanceModule::canTransitSuccessState() BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan() { - // stop_watch_.tic(std::string(__func__)); - const auto & input_path = getPreviousModuleOutput().path; if (input_path.points.empty()) { throw std::runtime_error("input path is empty"); @@ -440,11 +454,6 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan() output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; output.modified_goal = getPreviousModuleOutput().modified_goal; - // const double calculation_time = stop_watch_.toc(std::string(__func__)); - // RCLCPP_INFO_STREAM_EXPRESSION( - // getLogger(), parameters_->enable_debug_info, __func__ << ":=" << calculation_time << " - // [ms]"); - return output; } @@ -494,7 +503,10 @@ ObjectType DynamicObstacleAvoidanceModule::getObjectType(const uint8_t label) co void DynamicObstacleAvoidanceModule::registerRegulatedObjects( const std::vector & prev_objects) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto input_path = getPreviousModuleOutput().path; + const auto input_points = toGeometryPoints(input_path.points); // for efficient computation const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { @@ -508,6 +520,10 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( predicted_object.kinematics.predicted_paths.begin(), predicted_object.kinematics.predicted_paths.end(), [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + const size_t obj_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(input_points, obj_pose.position); + const size_t obj_idx = + getNearestIndexFromSegmentIndex(input_points, obj_seg_idx, obj_pose.position); // 1.a. check label if (getObjectType(predicted_object.classification.front().label) != ObjectType::REGULATED) { @@ -516,7 +532,7 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = - projectObstacleVelocityToTrajectory(input_path.points, predicted_object); + projectObstacleVelocityToTrajectory(input_path.points, predicted_object, obj_idx); if ( std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { @@ -524,7 +540,7 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleAgainstPath(input_path.points, obj_pose); + const double obj_angle = calcDiffAngleAgainstPath(input_path.points, obj_pose, obj_idx); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -544,7 +560,7 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( } // 1.e. check if object lateral offset to ego's path is small enough - const double obj_dist_to_path = calcDistanceToPath(input_path.points, obj_pose.position); + const double obj_dist_to_path = calcDistanceToPath(input_points, obj_pose.position, obj_idx); const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); if (is_object_far_from_path) { RCLCPP_INFO_EXPRESSION( @@ -582,7 +598,10 @@ void DynamicObstacleAvoidanceModule::registerRegulatedObjects( void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( const std::vector & prev_objects) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto input_path = getPreviousModuleOutput().path; + const auto input_points = toGeometryPoints(input_path.points); // for efficient computation const auto & predicted_objects = planner_data_->dynamic_object->objects; for (const auto & predicted_object : predicted_objects) { @@ -596,6 +615,10 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( predicted_object.kinematics.predicted_paths.begin(), predicted_object.kinematics.predicted_paths.end(), [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + const size_t obj_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(input_points, obj_pose.position); + const size_t obj_idx = + getNearestIndexFromSegmentIndex(input_points, obj_seg_idx, obj_pose.position); // 1.a. Check if the obstacle is labeled as pedestrians, bicycle or similar. if (getObjectType(predicted_object.classification.front().label) != ObjectType::UNREGULATED) { @@ -604,7 +627,7 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( // 1.b. Check if the object's velocity is within the module's coverage range. const auto [obj_tangent_vel, obj_normal_vel] = - projectObstacleVelocityToTrajectory(input_path.points, predicted_object); + projectObstacleVelocityToTrajectory(input_path.points, predicted_object, obj_idx); if ( obj_vel_norm < parameters_->min_obstacle_vel || obj_vel_norm > parameters_->max_obstacle_vel) { @@ -626,7 +649,7 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( // 1.f. calculate the object is on ego's path or not const double dist_obj_center_to_path = - std::abs(autoware::motion_utils::calcLateralOffset(input_path.points, obj_pose.position)); + std::abs(autoware::motion_utils::calcLateralOffset(input_points, obj_pose.position)); const bool is_object_on_ego_path = dist_obj_center_to_path < planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path; @@ -656,7 +679,10 @@ void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects( const std::vector & prev_objects) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto & input_path = getPreviousModuleOutput().path; + const auto input_points = toGeometryPoints(input_path.points); // for efficient computation for (const auto & object : target_objects_manager_.getValidObjects()) { if (getObjectType(object.label) != ObjectType::REGULATED) { @@ -668,11 +694,15 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje const auto obj_path = *std::max_element( object.predicted_paths.begin(), object.predicted_paths.end(), [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + const size_t obj_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(input_points, object.pose.position); + const size_t obj_idx = + getNearestIndexFromSegmentIndex(input_points, obj_seg_idx, object.pose.position); - const auto & ref_path_points_for_obj_poly = input_path.points; + const auto & ref_points_for_obj_poly = input_points; // 2.a. check if object is not to be followed by ego - const double obj_angle = calcDiffAngleAgainstPath(input_path.points, object.pose); + const double obj_angle = calcDiffAngleAgainstPath(input_path.points, object.pose, obj_idx); const bool is_object_aligned_to_path = std::abs(obj_angle) < parameters_->max_front_object_angle || M_PI - parameters_->max_front_object_angle < std::abs(obj_angle); @@ -686,9 +716,9 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje } // 2.b. calculate which side object exists against ego's path - const bool is_object_left = isLeft(input_path.points, object.pose.position); + const bool is_object_left = isLeft(input_path.points, object.pose.position, obj_idx); const auto lat_lon_offset = - getLateralLongitudinalOffset(input_path.points, object.pose, object.shape); + getLateralLongitudinalOffset(input_points, object.pose, obj_seg_idx, object.shape); // 2.c. check if object will not cut in const bool will_object_cut_in = @@ -759,7 +789,10 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje } const auto future_obj_pose = object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); - return future_obj_pose ? isLeft(input_path.points, future_obj_pose->position) + const size_t future_obj_idx = + autoware::motion_utils::findNearestIndex(input_path.points, future_obj_pose->position); + + return future_obj_pose ? isLeft(input_path.points, future_obj_pose->position, future_obj_idx) : is_object_left; }(); @@ -790,10 +823,10 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje // "ego_path_base" const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( - ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, + ref_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_while_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoidRegulatedObject( - ref_path_points_for_obj_poly, obj_points, object.pose.position, object.vel, is_collision_left, + ref_points_for_obj_poly, obj_points, object.pose.position, object.vel, is_collision_left, object.lat_vel, prev_object); if (!lat_offset_to_avoid) { @@ -808,7 +841,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje const bool should_be_avoided = true; target_objects_manager_.updateObjectVariables( obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided, - ref_path_points_for_obj_poly); + ref_points_for_obj_poly); } // prev_input_ref_path_points_ = input_ref_path_points; } @@ -816,23 +849,31 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObje void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects( const std::vector & prev_objects) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto & input_path = getPreviousModuleOutput().path; + const auto input_points = toGeometryPoints(input_path.points); // for efficient computation + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); for (const auto & object : target_objects_manager_.getValidObjects()) { if (getObjectType(object.label) != ObjectType::UNREGULATED) { continue; } + const size_t obj_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(input_points, object.pose.position); const auto obj_uuid = object.uuid; - const auto & ref_path_points_for_obj_poly = input_path.points; + const auto & ref_points_for_obj_poly = input_points; // 2.g. check if the ego is not ahead of the object. + time_keeper_->start_track("getLateralLongitudinalOffset"); const auto lat_lon_offset = - getLateralLongitudinalOffset(input_path.points, object.pose, object.shape); + getLateralLongitudinalOffset(input_points, object.pose, obj_seg_idx, object.shape); + time_keeper_->end_track("getLateralLongitudinalOffset"); + const double signed_dist_ego_to_obj = [&]() { - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); const double lon_offset_ego_to_obj = autoware::motion_utils::calcSignedArcLength( - input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + input_points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + @@ -854,7 +895,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedOb // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by // "ego_path_base" const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoidUnregulatedObject( - ref_path_points_for_obj_poly, getObstacleFromUuid(prev_objects, obj_uuid), object); + ref_points_for_obj_poly, getObstacleFromUuid(prev_objects, obj_uuid), object); if (!lat_offset_to_avoid) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -870,7 +911,7 @@ void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedOb const bool should_be_avoided = true; target_objects_manager_.updateObjectVariables( obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided, - ref_path_points_for_obj_poly); + ref_points_for_obj_poly); } } @@ -1186,30 +1227,54 @@ DynamicObstacleAvoidanceModule::getAdjacentLanes( DynamicObstacleAvoidanceModule::LatLonOffset DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( - const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const std::vector & ego_points, + const geometry_msgs::msg::Pose & obj_pose, const size_t obj_seg_idx, const autoware_perception_msgs::msg::Shape & obj_shape) const { - const size_t obj_seg_idx = - autoware::motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); - const auto obj_points = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); - // TODO(murooka) calculation is not so accurate. std::vector obj_lat_offset_vec; std::vector obj_lon_offset_vec; - for (size_t i = 0; i < obj_points.outer().size(); ++i) { - const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const size_t obj_point_seg_idx = - autoware::motion_utils::findNearestSegmentIndex(ego_path, geom_obj_point); + if (obj_shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + // NOTE: efficient calculation for the CYLINDER object. + const double radius = obj_shape.dimensions.x / 2.0; // calculate lateral offset - const double obj_point_lat_offset = - autoware::motion_utils::calcLateralOffset(ego_path, geom_obj_point, obj_point_seg_idx); - obj_lat_offset_vec.push_back(obj_point_lat_offset); + const double obj_lat_offset = + autoware::motion_utils::calcLateralOffset(ego_points, obj_pose.position, obj_seg_idx); + double obj_max_lat_offset = obj_lat_offset + radius; + if (obj_max_lat_offset * obj_lat_offset < 0) { + obj_max_lat_offset = 0.0; + } + double obj_min_lat_offset = obj_lat_offset - radius; + if (obj_min_lat_offset * obj_lat_offset < 0) { + obj_min_lat_offset = 0.0; + } + + obj_lat_offset_vec.push_back(obj_max_lat_offset); + obj_lat_offset_vec.push_back(obj_min_lat_offset); // calculate longitudinal offset - const double lon_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - ego_path, obj_seg_idx, geom_obj_point); - obj_lon_offset_vec.push_back(lon_offset); + const double obj_lon_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( + ego_points, obj_seg_idx, obj_pose.position); + obj_lon_offset_vec.push_back(obj_lon_offset - radius); + obj_lon_offset_vec.push_back(obj_lon_offset + radius); + } else { + const auto obj_points = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const size_t obj_point_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(ego_points, geom_obj_point); + + // calculate lateral offset + const double obj_point_lat_offset = + autoware::motion_utils::calcLateralOffset(ego_points, geom_obj_point, obj_point_seg_idx); + obj_lat_offset_vec.push_back(obj_point_lat_offset); + + // calculate longitudinal offset + const double lon_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( + ego_points, obj_seg_idx, geom_obj_point); + obj_lon_offset_vec.push_back(lon_offset); + } } const auto obj_lat_min_max_offset = getMinMaxValues(obj_lat_offset_vec); @@ -1221,13 +1286,13 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( } MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & ref_path_points_for_obj_poly, + const std::vector & ref_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape, const TimeWhileCollision & time_while_collision) const { - const size_t obj_seg_idx = autoware::motion_utils::findNearestSegmentIndex( - ref_path_points_for_obj_poly, obj_pose.position); + const size_t obj_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(ref_points_for_obj_poly, obj_pose.position); // calculate min/max longitudinal offset from object to path const auto obj_lon_offset = [&]() { @@ -1235,7 +1300,7 @@ MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const double lon_offset = autoware::motion_utils::calcLongitudinalOffsetToSegment( - ref_path_points_for_obj_poly, obj_seg_idx, geom_obj_point); + ref_points_for_obj_poly, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } @@ -1388,22 +1453,21 @@ double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( // min value denotes near side, max value denotes far side std::optional DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( - const std::vector & ref_path_points_for_obj_poly, + const std::vector & ref_points_for_obj_poly, const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, const bool is_collision_left, const double obj_normal_vel, const std::optional & prev_object) const { const bool enable_lowpass_filter = [&]() { if ( - !prev_object || prev_object->ref_path_points_for_obj_poly.size() < 2 || - ref_path_points_for_obj_poly.size() < 2) { + !prev_object || prev_object->ref_points_for_obj_poly.size() < 2 || + ref_points_for_obj_poly.size() < 2) { return true; } const size_t obj_point_idx = - autoware::motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); + autoware::motion_utils::findNearestIndex(ref_points_for_obj_poly, obj_pos); const double paths_lat_diff = std::abs(autoware::motion_utils::calcLateralOffset( - prev_object->ref_path_points_for_obj_poly, - ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); + prev_object->ref_points_for_obj_poly, ref_points_for_obj_poly.at(obj_point_idx).position)); constexpr double min_paths_lat_diff = 0.3; if (paths_lat_diff < min_paths_lat_diff) { @@ -1419,10 +1483,10 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( std::vector obj_lat_abs_offset_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const size_t obj_point_seg_idx = autoware::motion_utils::findNearestSegmentIndex( - ref_path_points_for_obj_poly, geom_obj_point); + const size_t obj_point_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(ref_points_for_obj_poly, geom_obj_point); const double obj_point_lat_offset = autoware::motion_utils::calcLateralOffset( - ref_path_points_for_obj_poly, geom_obj_point, obj_point_seg_idx); + ref_points_for_obj_poly, geom_obj_point, obj_point_seg_idx); obj_lat_abs_offset_vec.push_back(obj_point_lat_offset); } return getMinMaxValues(obj_lat_abs_offset_vec); @@ -1483,21 +1547,28 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( // min value denotes near side, max value denotes far side std::optional DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( - const std::vector & ref_path_points_for_obj_poly, + const std::vector & ref_points_for_obj_poly, const std::optional & prev_object, const DynamicAvoidanceObject & object) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + time_keeper_->start_track("findNearestSegmentIndex of object position"); + const size_t obj_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(ref_points_for_obj_poly, object.pose.position); + time_keeper_->end_track("findNearestSegmentIndex of object position"); + const size_t obj_point_idx = + getNearestIndexFromSegmentIndex(ref_points_for_obj_poly, obj_seg_idx, object.pose.position); + const bool enable_lowpass_filter = [&]() { + universe_utils::ScopedTimeTrack st("calc enable_lowpass_filter", *time_keeper_); if ( - !prev_object || prev_object->ref_path_points_for_obj_poly.size() < 2 || - ref_path_points_for_obj_poly.size() < 2) { + !prev_object || prev_object->ref_points_for_obj_poly.size() < 2 || + ref_points_for_obj_poly.size() < 2) { return true; } - const size_t obj_point_idx = - autoware::motion_utils::findNearestIndex(ref_path_points_for_obj_poly, object.pose.position); const double paths_lat_diff = std::abs(autoware::motion_utils::calcLateralOffset( - prev_object->ref_path_points_for_obj_poly, - ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); + prev_object->ref_points_for_obj_poly, ref_points_for_obj_poly.at(obj_point_idx).position)); constexpr double min_paths_lat_diff = 0.3; if (paths_lat_diff < min_paths_lat_diff) { @@ -1509,15 +1580,33 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( }(); const auto obj_occupancy_region = [&]() { - const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); + universe_utils::ScopedTimeTrack st("calc obj_occupancy_region", *time_keeper_); std::vector lat_pos_vec; - for (size_t i = 0; i < obj_points.outer().size(); ++i) { - const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const double obj_point_lat_offset = autoware::motion_utils::calcLateralOffset( - ref_path_points_for_obj_poly, geom_obj_point, - autoware::motion_utils::findNearestSegmentIndex( - ref_path_points_for_obj_poly, geom_obj_point)); - lat_pos_vec.push_back(obj_point_lat_offset); + if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + // NOTE: efficient calculation for the CYLINDER object. + const double radius = object.shape.dimensions.x / 2.0; + + const double obj_lat_offset = autoware::motion_utils::calcLateralOffset( + ref_points_for_obj_poly, object.pose.position, obj_seg_idx); + const double obj_min_lat_offset = [&]() { + if (std::abs(obj_lat_offset) < radius) { + return 0.0; + } + if (0.0 < obj_lat_offset) { + return obj_lat_offset - radius; + } + return obj_lat_offset + radius; + }(); + lat_pos_vec.push_back(obj_min_lat_offset); + } else { + const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const double obj_point_lat_offset = autoware::motion_utils::calcLateralOffset( + ref_points_for_obj_poly, geom_obj_point, + autoware::motion_utils::findNearestSegmentIndex(ref_points_for_obj_poly, geom_obj_point)); + lat_pos_vec.push_back(obj_point_lat_offset); + } } const auto current_pos_area = getMinMaxValues(lat_pos_vec); return combineMinMaxValues(current_pos_area, current_pos_area + object.lat_vel * 3.0); @@ -1574,19 +1663,19 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( return std::nullopt; } - auto ref_path_points_for_obj_poly = object.ref_path_points_for_obj_poly; + auto ref_points_for_obj_poly = object.ref_points_for_obj_poly; - const size_t obj_seg_idx = autoware::motion_utils::findNearestSegmentIndex( - ref_path_points_for_obj_poly, object.pose.position); + const size_t obj_seg_idx = + autoware::motion_utils::findNearestSegmentIndex(ref_points_for_obj_poly, object.pose.position); // const auto obj_points = autoware::universe_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = autoware::motion_utils::insertTargetPoint( - obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_path_points_for_obj_poly); + obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_points_for_obj_poly); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = autoware::motion_utils::insertTargetPoint( - updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, ref_path_points_for_obj_poly); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, ref_points_for_obj_poly); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -1596,15 +1685,14 @@ DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(ref_path_points_for_obj_poly.size() - 1); + : static_cast(ref_points_for_obj_poly.size() - 1); // create inner bound points std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. obj_inner_bound_poses.push_back(autoware::universe_utils::calcOffsetPose( - ref_path_points_for_obj_poly.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, - 0.0)); + ref_points_for_obj_poly.at(i), 0.0, object.lat_offset_to_avoid->min_value, 0.0)); } // calculate start index laterally feasible for ego to shift considering maximum lateral jerk and @@ -1797,9 +1885,6 @@ DynamicObstacleAvoidanceModule::EgoPathReservePoly DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & ego_path) const { // This function require almost 0.5 ms. Should be refactored in the future - // double calculation_time; - // stop_watch_.tic(std::string(__func__)); - namespace strategy = boost::geometry::strategy::buffer; assert(!ego_path.points.empty()); @@ -1862,10 +1947,6 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg strategy::distance_asymmetric(0.0, vehicle_half_width), motion_saturated_outer_paths.left_path); - // calculation_time = stop_watch_.toc(std::string(__func__)); - // RCLCPP_INFO_STREAM_EXPRESSION( - // getLogger(), parameters_->enable_debug_info, __func__ << ":=" << calculation_time << "[ms]"); - return {left_avoid_poly, right_avoid_poly}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 3e155aba0af2e..014aaf6db6657 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -173,20 +173,19 @@ The gray numbers represent objects to avoid, and you can see that the goal in fr ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | -| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | -| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | -| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | +| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | This parameter ensures that the distance between the start of the shoulder lane and the goal is not less than the specified value. It's used to prevent setting goals too close to the beginning of the shoulder lane, which might lead to unsafe or impractical pull-over maneuvers. Increasing this value will force the system to ignore potential goal positions near the start of the shoulder lane, potentially leading to safer and more comfortable pull-over locations. | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 53e06631a81d5..e6f84e7eece0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -22,6 +22,7 @@ lateral_offset_interval: 0.25 ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 + high_curvature_threshold: 0.1 # occupancy grid map occupancy_grid: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index bdb267ce223d3..6a10c0fb183f6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -60,6 +60,7 @@ struct GoalPlannerParameters double lateral_offset_interval{0.0}; double ignore_distance_from_lane_start{0.0}; double margin_from_boundary{0.0}; + double high_curvature_threshold{0.0}; // occupancy grid map bool use_occupancy_grid_for_goal_search{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp index d2ba416c7fa90..05385600926f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -52,6 +52,7 @@ struct PullOverPath size_t goal_id{}; size_t id{}; bool decided_velocity{false}; + double max_curvature{0.0}; // max curvature of the parking path PathWithLaneId getFullPath() const { @@ -72,6 +73,7 @@ struct PullOverPath return path; } + // path from the pull over start pose to the end pose PathWithLaneId getParkingPath() const { const PathWithLaneId full_path = getFullPath(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp index ee35790ee2ab1..1f79f330be676 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -122,8 +122,8 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); // Check if driving forward for each path, return empty if not - for (auto & path : partial_paths) { - if (!autoware::motion_utils::isDrivingForward(path.points)) { + for (auto & partial_path : partial_paths) { + if (!autoware::motion_utils::isDrivingForward(partial_path.points)) { return {}; } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 00510a8725e32..1a711adb133eb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -293,9 +293,18 @@ void GoalPlannerModule::onTimer() planner->setPlannerData(local_planner_data); planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path) { + if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) { pull_over_path->goal_id = goal_candidate.id; pull_over_path->id = path_candidates.size(); + + // calculate absolute maximum curvature of parking path(start pose to end pose) for path + // priority + const std::vector curvatures = + autoware::motion_utils::calcCurvature(pull_over_path->getParkingPath().points); + pull_over_path->max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -817,23 +826,37 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto debugPrintPathPriority = [this]( const std::vector & sorted_pull_over_path_candidates, - const std::map & goal_id_to_index, - const std::optional> & path_id_to_margin_map_opt = std::nullopt, - const std::optional> & isSoftMarginOpt = - std::nullopt) { + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature) { std::stringstream ss; - ss << "\n---------------------- path priority ----------------------\n"; - for (const auto & path : sorted_pull_over_path_candidates) { - // clang-format off - ss << "path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id - << ", goal_id: " << path.goal_id - << ", goal_priority:" << goal_id_to_index.at(path.goal_id); - // clang-format on - if (path_id_to_margin_map_opt && isSoftMarginOpt) { - ss << ", margin: " << path_id_to_margin_map_opt->at(path.id) - << ((*isSoftMarginOpt)(path) ? " (soft)" : " (hard)"); + + // unsafe goal and it's priority are not visible as debug marker in rviz, + // so exclude unsafe goal from goal_priority + std::map goal_id_and_priority; + { + int priority = 0; + for (const auto & goal_candidate : goal_candidates) { + goal_id_and_priority[goal_candidate.id] = goal_candidate.is_safe ? priority++ : -1; } + } + + ss << "\n---------------------- path priority ----------------------\n"; + for (size_t i = 0; i < sorted_pull_over_path_candidates.size(); ++i) { + const auto & path = sorted_pull_over_path_candidates[i]; + + // goal_index is same to goal priority including unsafe goal + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); + const bool is_safe_goal = goal_candidates[goal_index].is_safe; + const int goal_priority = goal_id_and_priority[path.goal_id]; + + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) + << ", path_id: " << path.id << ", goal_id: " << path.goal_id + << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") + << ", margin: " << path_id_to_margin_map.at(path.id) + << (isSoftMargin(path) ? " (soft)" : " (hard)") << ", curvature: " << path.max_curvature + << (isHighCurvature(path) ? " (high)" : " (low)"); ss << "\n"; } ss << "-----------------------------------------------------------\n"; @@ -844,6 +867,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins; const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins; + // (1) Sort pull_over_path_candidates based on the order in goal_candidates // Create a map of goal_id to its index in goal_candidates std::map goal_id_to_index; for (size_t i = 0; i < goal_candidates.size(); ++i) { @@ -868,6 +892,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri // if object recognition is enabled, sort by collision check margin if (parameters_->use_object_recognition) { + // (2) Sort by collision check margins const std::vector margins = std::invoke([&]() { std::vector combined_margins = soft_margins; combined_margins.insert(combined_margins.end(), hard_margins.begin(), hard_margins.end()); @@ -914,23 +939,45 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; }); - // Sort pull_over_path_candidates based on the order in efficient_path_order - if (parameters_->path_priority == "efficient_path") { - const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_margin_map[path.id]; - return std::any_of( - soft_margins.begin(), soft_margins.end(), - [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); - }; - const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { - return !isSoftMargin(a) && !isSoftMargin(b) && - std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01; - }; + // (3) Sort by curvature + // If the curvature is less than the threshold, prioritize the path. + const auto isHighCurvature = [&](const PullOverPath & path) -> bool { + return path.max_curvature >= parameters_->high_curvature_threshold; + }; + const auto isSoftMargin = [&](const PullOverPath & path) -> bool { + const double margin = path_id_to_margin_map[path.id]; + return std::any_of( + soft_margins.begin(), soft_margins.end(), + [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); + }; + const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { + return !isSoftMargin(a) && !isSoftMargin(b) && + std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01; + }; + + // NOTE: this is just partition sort based on curvature threshold within each sub partitions + std::stable_sort( + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), + [&](const PullOverPath & a, const PullOverPath & b) { + // if both are soft margin or both are same hard margin, prioritize the path with lower + // curvature. + if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { + return !isHighCurvature(a) && isHighCurvature(b); + } + // otherwise, keep the order based on the margin. + return false; + }); + + // (4) Sort pull_over_path_candidates based on the order in efficient_path_order keeping the + // collision check margin and curvature priority. + if (parameters_->path_priority == "efficient_path") { std::stable_sort( sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [&](const auto & a, const auto & b) { - // if both are soft margin or both are same hard margin, sort by planner priority + // if any of following conditions are met, sort by path type priority + // - both are soft margin + // - both are same hard margin if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { return comparePathTypePriority(a, b); } @@ -938,18 +985,24 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return false; }); - // debug print path priority: sorted by efficient_path_order and collision check margin + // debug print path priority sorted by + // - efficient_path_order + // - collision check margin + // - curvature debugPrintPathPriority( - sorted_pull_over_path_candidates, goal_id_to_index, path_id_to_margin_map, isSoftMargin); + sorted_pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_margin_map, + isSoftMargin, isHighCurvature); } } else { - // Sort pull_over_path_candidates based on the order in efficient_path_order + /** + * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the + * future. sort by curvature is not implemented yet. + * Sort pull_over_path_candidates based on the order in efficient_path_order + */ if (parameters_->path_priority == "efficient_path") { std::stable_sort( sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [&](const auto & a, const auto & b) { return comparePathTypePriority(a, b); }); - // debug print path priority: sorted by efficient_path_order and collision check margin - debugPrintPathPriority(sorted_pull_over_path_candidates, goal_id_to_index); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index b079db9babf31..b86bbbca7d98b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -61,6 +61,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.ignore_distance_from_lane_start = node->declare_parameter(ns + "ignore_distance_from_lane_start"); p.margin_from_boundary = node->declare_parameter(ns + "margin_from_boundary"); + p.high_curvature_threshold = node->declare_parameter(ns + "high_curvature_threshold"); const std::string parking_policy_name = node->declare_parameter(ns + "parking_policy"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 3d2bf9500bc78..9570621f0f717 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -308,8 +308,19 @@ double ShiftPullOver::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : - autoware::motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + + const auto curvature_and_segment_length = + autoware::motion_utils::calcCurvatureAndSegmentLength(reversed_path.points); + + for (size_t i = 0; i < curvature_and_segment_length.size(); ++i) { + const auto & [k, segment_length_pair] = curvature_and_segment_length[i]; + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvature_and_segment_length.size() - 1 + ? segment_length_pair.first + : segment_length_pair.first + segment_length_pair.second; + // after shifted segment length const double after_segment_length = k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index d6f03f79186d8..b74cdbc6a569b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -145,8 +145,8 @@ PredictedObjects extractObjectsInExpandedPullOverLanes( route_handler, left_side, backward_distance, forward_distance, bound_offset); const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( - objects, lanes, [](const auto & obj, const auto & lanelet) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet); + objects, lanes, [](const auto & obj, const auto & lanelet, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet, yaw_threshold); }); return objects_in_lanes; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 9c014b77a7e82..bd309dd35a260 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include #include #include @@ -108,6 +109,7 @@ class LaneChangeInterface : public SceneModuleInterface const double start_distance, const double finish_distance, const bool safe, const uint8_t & state) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { if (ptr) { ptr->updateCooperateStatus( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index be62492b7c2cc..dad96d5f7371a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -51,6 +51,8 @@ class NormalLaneChange : public LaneChangeBase NormalLaneChange & operator=(NormalLaneChange &&) = delete; ~NormalLaneChange() override = default; + void update_lanes(const bool is_approved) final; + void updateLaneChangeStatus() override; std::pair getSafePath(LaneChangePath & safe_path) const override; @@ -105,8 +107,6 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() override; protected: - lanelet::ConstLanelets getCurrentLanes() const override; - lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const override; @@ -124,9 +124,7 @@ class NormalLaneChange : public LaneChangeBase const LaneChangeLanesFilteredObjects & predicted_objects, const lanelet::ConstLanelets & current_lanes) const; - LaneChangeLanesFilteredObjects filterObjects( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + LaneChangeLanesFilteredObjects filterObjects() const; void filterOncomingObjects(PredictedObjects & objects) const; @@ -203,6 +201,11 @@ class NormalLaneChange : public LaneChangeBase double getStopTime() const { return stop_time_; } + const lanelet::ConstLanelets & get_target_lanes() const + { + return common_data_ptr_->lanes_ptr->target; + } + double stop_time_{0.0}; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index e8e83a0703a76..c1cc14d98a7c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include #include #include @@ -50,7 +51,11 @@ class LaneChangeBase LaneChangeBase( std::shared_ptr parameters, LaneChangeModuleType type, Direction direction) - : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type} + : lane_change_parameters_{std::move(parameters)}, + common_data_ptr_{std::make_shared()}, + direction_{direction}, + type_{type}, + time_keeper_(std::make_shared()) { } @@ -60,6 +65,8 @@ class LaneChangeBase LaneChangeBase & operator=(LaneChangeBase &&) = delete; virtual ~LaneChangeBase() = default; + virtual void update_lanes(const bool is_approved) = 0; + virtual void updateLaneChangeStatus() = 0; virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; @@ -134,6 +141,11 @@ class LaneChangeBase const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } + const lanelet::ConstLanelets & get_current_lanes() const + { + return common_data_ptr_->lanes_ptr->current; + } + const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; } LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; } @@ -151,7 +163,33 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } - void setData(const std::shared_ptr & data) { planner_data_ = data; } + void setData(const std::shared_ptr & data) + { + planner_data_ = data; + if (!common_data_ptr_->bpp_param_ptr) { + common_data_ptr_->bpp_param_ptr = + std::make_shared(data->parameters); + } + + if (!common_data_ptr_->lanes_ptr) { + common_data_ptr_->lanes_ptr = std::make_shared(); + } + + if (!common_data_ptr_->lanes_polygon_ptr) { + common_data_ptr_->lanes_polygon_ptr = std::make_shared(); + } + + common_data_ptr_->self_odometry_ptr = data->self_odometry; + common_data_ptr_->route_handler_ptr = data->route_handler; + common_data_ptr_->lc_param_ptr = lane_change_parameters_; + common_data_ptr_->lc_type = type_; + common_data_ptr_->direction = direction_; + } + + void setTimeKeeper(const std::shared_ptr & time_keeper) + { + time_keeper_ = time_keeper; + } void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; } @@ -190,8 +228,6 @@ class LaneChangeBase virtual TurnSignalInfo get_current_turn_signal_info() = 0; protected: - virtual lanelet::ConstLanelets getCurrentLanes() const = 0; - virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; virtual PathWithLaneId getPrepareSegment( @@ -219,6 +255,7 @@ class LaneChangeBase std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; + lane_change::CommonDataPtr common_data_ptr_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; @@ -237,6 +274,8 @@ class LaneChangeBase rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index ffd2754acc38f..1bb4dfdeb59dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -17,16 +17,28 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include +#include #include +#include + #include #include +#include #include #include -namespace autoware::behavior_path_planner +namespace autoware::behavior_path_planner::lane_change { +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using route_handler::Direction; +using route_handler::RouteHandler; +using utils::path_safety_checker::ExtendedPredictedObjects; + struct LateralAccelerationMap { std::vector base_vel; @@ -68,7 +80,7 @@ struct LateralAccelerationMap } }; -struct LaneChangeCancelParameters +struct CancelParameters { bool enable_on_prepare_phase{true}; bool enable_on_lane_changing_phase{false}; @@ -83,7 +95,7 @@ struct LaneChangeCancelParameters int unsafe_hysteresis_threshold{2}; }; -struct LaneChangeParameters +struct Parameters { // trajectory generation double backward_lane_length{200.0}; @@ -92,8 +104,8 @@ struct LaneChangeParameters int lateral_acc_sampling_num{10}; // lane change parameters - double backward_length_buffer_for_end_of_lane; - double backward_length_buffer_for_blocking_object; + double backward_length_buffer_for_end_of_lane{0.0}; + double backward_length_buffer_for_blocking_object{0.0}; double lane_changing_lateral_jerk{0.5}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; @@ -143,7 +155,7 @@ struct LaneChangeParameters utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort - LaneChangeCancelParameters cancel{}; + CancelParameters cancel{}; double finish_judge_lateral_threshold{0.2}; @@ -151,35 +163,39 @@ struct LaneChangeParameters bool publish_debug_marker{false}; }; -enum class LaneChangeStates { +enum class States { Normal = 0, Cancel, Abort, Stop, }; -struct LaneChangePhaseInfo +struct PhaseInfo { double prepare{0.0}; double lane_changing{0.0}; [[nodiscard]] double sum() const { return prepare + lane_changing; } - LaneChangePhaseInfo(const double _prepare, const double _lane_changing) + PhaseInfo(const double _prepare, const double _lane_changing) : prepare(_prepare), lane_changing(_lane_changing) { } }; -struct LaneChangeInfo +struct Lanes { - LaneChangePhaseInfo longitudinal_acceleration{0.0, 0.0}; - LaneChangePhaseInfo velocity{0.0, 0.0}; - LaneChangePhaseInfo duration{0.0, 0.0}; - LaneChangePhaseInfo length{0.0, 0.0}; + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; +struct Info +{ + PhaseInfo longitudinal_acceleration{0.0, 0.0}; + PhaseInfo velocity{0.0, 0.0}; + PhaseInfo duration{0.0, 0.0}; + PhaseInfo length{0.0, 0.0}; Pose lane_changing_start{}; Pose lane_changing_end{}; @@ -190,22 +206,19 @@ struct LaneChangeInfo double terminal_lane_changing_velocity{0.0}; }; -struct LaneChangeLanesFilteredObjects +struct LanesObjects { - utils::path_safety_checker::ExtendedPredictedObjects current_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects target_lane{}; - utils::path_safety_checker::ExtendedPredictedObjects other_lane{}; + ExtendedPredictedObjects current_lane{}; + ExtendedPredictedObjects target_lane{}; + ExtendedPredictedObjects other_lane{}; }; -enum class LaneChangeModuleType { +enum class ModuleType { NORMAL = 0, EXTERNAL_REQUEST, AVOIDANCE_BY_LANE_CHANGE, }; -} // namespace autoware::behavior_path_planner -namespace autoware::behavior_path_planner::lane_change -{ struct PathSafetyStatus { bool is_safe{true}; @@ -216,8 +229,55 @@ struct LanesPolygon { std::optional current; std::optional target; - std::vector target_backward; + std::optional expanded_target; + lanelet::BasicPolygon2d target_neighbor; + std::vector preceding_target; +}; + +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; +using LanesPolygonPtr = std::shared_ptr; + +struct CommonData +{ + RouteHandlerPtr route_handler_ptr; + Odometry::ConstSharedPtr self_odometry_ptr; + BppParamPtr bpp_param_ptr; + LCParamPtr lc_param_ptr; + LanesPtr lanes_ptr; + LanesPolygonPtr lanes_polygon_ptr; + ModuleType lc_type; + Direction direction; + + [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } + + [[nodiscard]] Twist get_ego_twist() const { return self_odometry_ptr->twist.twist; } + + [[nodiscard]] double get_ego_speed(bool use_norm = false) const + { + if (!use_norm) { + return get_ego_twist().linear.x; + } + + const auto x = get_ego_twist().linear.x; + const auto y = get_ego_twist().linear.y; + return std::hypot(x, y); + } }; +using CommonDataPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change +namespace autoware::behavior_path_planner +{ +using LaneChangeModuleType = lane_change::ModuleType; +using LaneChangeParameters = lane_change::Parameters; +using LaneChangeStates = lane_change::States; +using LaneChangePhaseInfo = lane_change::PhaseInfo; +using LaneChangeInfo = lane_change::Info; +using LaneChangeLanesFilteredObjects = lane_change::LanesObjects; +using LateralAccelerationMap = lane_change::LateralAccelerationMap; +} // namespace autoware::behavior_path_planner + #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 77c603e3bd975..5623f03a22eb3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -23,31 +23,31 @@ #include -namespace autoware::behavior_path_planner +namespace autoware::behavior_path_planner::lane_change { using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; -struct LaneChangePath +struct Path { PathWithLaneId path{}; ShiftedPath shifted_path{}; - PathWithLaneId prev_path{}; - LaneChangeInfo info{}; + Info info{}; }; -using LaneChangePaths = std::vector; -struct LaneChangeStatus +struct Status { - PathWithLaneId lane_follow_path{}; - LaneChangePath lane_change_path{}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; - std::vector lane_follow_lane_ids{}; - std::vector lane_change_lane_ids{}; + Path lane_change_path{}; bool is_safe{false}; bool is_valid_path{false}; double start_distance{0.0}; }; +} // namespace autoware::behavior_path_planner::lane_change + +namespace autoware::behavior_path_planner +{ +using LaneChangePath = lane_change::Path; +using LaneChangePaths = std::vector; +using LaneChangeStatus = lane_change::Status; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index e25b67bdb73d8..17eaceb055fc1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -48,6 +48,7 @@ using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; @@ -114,8 +115,9 @@ bool isPathInLanelets( const lanelet::ConstLanelets & target_lanes); std::optional constructCandidatePath( - const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, - const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); ShiftLine getLaneChangingShiftLine( @@ -177,10 +179,9 @@ bool isParkedObject( const double ratio_threshold); bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, - CollisionCheckDebugMap & object_debug); + const bool is_goal_in_route, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, @@ -195,7 +196,8 @@ ExtendedPredictedObject transform( const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + const std::vector & collided_polygons, + const std::optional & lanes_polygon); /** * @brief Generates expanded lanelets based on the given direction and offsets. @@ -295,9 +297,11 @@ double calcPhaseLength( const double velocity, const double maximum_velocity, const double acceleration, const double time); -LanesPolygon createLanesPolygon( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const std::vector & target_backward_lanes); +LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); + +bool is_same_lane_with_prev_iteration( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 0a5b31f9e32d2..4823cb0bfec22 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -21,6 +21,7 @@ #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include +#include #include #include @@ -43,6 +44,7 @@ LaneChangeInterface::LaneChangeInterface( module_type_{std::move(module_type)}, prev_approved_path_{std::make_unique()} { + module_type_->setTimeKeeper(getTimeKeeper()); steering_factor_interface_ptr_ = std::make_unique(&node, name); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } @@ -71,7 +73,9 @@ bool LaneChangeInterface::isExecutionReady() const void LaneChangeInterface::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); + module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -94,6 +98,7 @@ void LaneChangeInterface::postProcess() BehaviorModuleOutput LaneChangeInterface::plan() { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); resetPathCandidate(); resetPathReference(); @@ -132,7 +137,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() *prev_approved_path_ = getPreviousModuleOutput().path; BehaviorModuleOutput out = getPreviousModuleOutput(); - module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); @@ -165,6 +170,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() CandidateOutput LaneChangeInterface::planCandidate() const { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); const auto selected_path = module_type_->getLaneChangePath(); if (selected_path.path.points.empty()) { @@ -340,6 +346,7 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); const auto steering_factor_direction = std::invoke([&]() { if (module_type_->getDirection() == Direction::LEFT) { return SteeringFactor::LEFT; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index f3e05eca1e92d..4dfba64615504 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include #include @@ -40,7 +41,7 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::calcMinimumLaneChangeLength; -using utils::lane_change::createLanesPolygon; +using utils::lane_change::create_lanes_polygon; using utils::path_safety_checker::isPolygonOverlapLanelet; using utils::traffic_light::getDistanceToNextTrafficLight; @@ -53,40 +54,66 @@ NormalLaneChange::NormalLaneChange( stop_watch_.tic("stop_time"); } +void NormalLaneChange::update_lanes(const bool is_approved) +{ + if (is_approved) { + return; + } + + const auto current_lanes = + utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); + + if (current_lanes.empty()) { + return; + } + + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + if (target_lanes.empty()) { + return; + } + + const auto is_same_lanes_with_prev_iteration = + utils::lane_change::is_same_lane_with_prev_iteration( + common_data_ptr_, current_lanes, target_lanes); + + if (is_same_lanes_with_prev_iteration) { + return; + } + + common_data_ptr_->lanes_ptr->current = current_lanes; + common_data_ptr_->lanes_ptr->target = target_lanes; + + common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( + *common_data_ptr_->route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), + common_data_ptr_->lc_param_ptr->backward_lane_length); + + *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); +} + void NormalLaneChange::updateLaneChangeStatus() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status - status_.current_lanes = status_.lane_change_path.info.current_lanes; - status_.target_lanes = status_.lane_change_path.info.target_lanes; status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.lane_change_lane_ids = utils::getIds(status_.target_lanes); - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.target_lanes, getEgoPose()); + const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); status_.start_distance = arclength_start.length; status_.lane_change_path.path.header = getRouteHeader(); } std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) const { - const auto current_lanes = getCurrentLanes(); - - if (current_lanes.empty()) { - return {false, false}; - } + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - - if (target_lanes.empty()) { + if (current_lanes.empty() || target_lanes.empty()) { return {false, false}; } - // find candidate paths LaneChangePaths valid_paths{}; const bool is_stuck = isVehicleStuck(current_lanes); bool found_safe_path = getLaneChangePaths( @@ -102,8 +129,6 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { - safe_path.info.current_lanes = current_lanes; - safe_path.info.target_lanes = target_lanes; return {false, false}; } @@ -119,21 +144,22 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool NormalLaneChange::isLaneChangeRequired() { - status_.current_lanes = getCurrentLanes(); + const auto current_lanes = + utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); - if (status_.current_lanes.empty()) { + if (current_lanes.empty()) { return false; } - status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - return !status_.target_lanes.empty(); + return !target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const { return utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - status_.current_lanes, status_.lane_change_path.path, planner_data_, + get_current_lanes(), status_.lane_change_path.path, planner_data_, status_.lane_change_path.info.length.sum()); } @@ -141,7 +167,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() { const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto & current_lanes = getLaneChangeStatus().current_lanes; + const auto & current_lanes = get_current_lanes(); const auto is_valid = getLaneChangeStatus().is_valid_path; const auto & lane_change_path = getLaneChangeStatus().lane_change_path; const auto & lane_change_param = getLaneChangeParam(); @@ -234,7 +260,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const return output; } - const auto current_lanes = getCurrentLanes(); + const auto & current_lanes = get_current_lanes(); if (current_lanes.empty()) { RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); return prev_module_output_; @@ -263,6 +289,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const BehaviorModuleOutput NormalLaneChange::generateOutput() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); return prev_module_output_; @@ -271,7 +298,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() auto output = prev_module_output_; if (isAbortState() && abort_path_) { output.path = abort_path_->path; - insertStopPoint(status_.current_lanes, output.path); + insertStopPoint(get_current_lanes(), output.path); } else { output.path = getLaneChangePath().path; @@ -291,7 +318,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); setStopPose(stop_point.point.pose); } else { - insertStopPoint(status_.target_lanes, output.path); + insertStopPoint(get_target_lanes(), output.path); } } @@ -308,10 +335,11 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *getRouteHandler(), status_.current_lanes, status_.target_lanes); + *getRouteHandler(), get_current_lanes(), get_target_lanes()); const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, @@ -327,6 +355,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c void NormalLaneChange::insertStopPoint( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (lanelets.empty()) { return; } @@ -356,15 +385,14 @@ void NormalLaneChange::insertStopPoint( } const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = filterObjects(status_.current_lanes, status_.target_lanes); + const auto target_objects = filterObjects(); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; const auto is_valid_start_point = std::invoke([&]() -> bool { auto lc_start_point = lanelet::utils::conversion::toLaneletPoint( status_.lane_change_path.info.lane_changing_start.position); - const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon( - *route_handler, status_.current_lanes, type_); + const auto & target_neighbor_preferred_lane_poly_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; return boost::geometry::covered_by( lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); }); @@ -444,7 +472,7 @@ void NormalLaneChange::insertStopPoint( // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(status_.target_lanes) + lanelet::utils::combineLaneletsShape(get_target_lanes()) .polygon2d() .basicPolygon())) { return false; @@ -468,9 +496,10 @@ void NormalLaneChange::insertStopPoint( PathWithLaneId NormalLaneChange::getReferencePath() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( - status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) { + get_target_lanes(), getEgoPose(), &closest_lanelet)) { return prev_module_output_.reference_path; } const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); @@ -482,6 +511,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const std::optional NormalLaneChange::extendPath() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto path = status_.lane_change_path.path; const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; @@ -491,7 +521,7 @@ std::optional NormalLaneChange::extendPath() return std::nullopt; } - auto & target_lanes = status_.target_lanes; + auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); @@ -560,8 +590,9 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & pose = getEgoPose(); - const auto & current_lanes = status_.current_lanes; + const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; @@ -576,14 +607,10 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const -{ - return utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); -} - lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { return {}; } @@ -657,8 +684,9 @@ bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; - const double dist_to_lane_change_end = utils::getSignedDistance( - current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); + const auto & target_lanes = get_target_lanes(); + const double dist_to_lane_change_end = + utils::getSignedDistance(current_pose, lane_change_end, get_target_lanes()); double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; // If ego velocity is low, relax finish judge buffer @@ -676,7 +704,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(status_.target_lanes, current_pose); + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -693,7 +721,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const } if (!utils::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters, + get_current_lanes(), getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; @@ -714,7 +742,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters, + get_current_lanes(), estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; return is_ego_within_original_lane; @@ -753,7 +781,7 @@ bool NormalLaneChange::isAbleToStopSafely() const if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters); + get_current_lanes(), estimated_pose, planner_data_->parameters); } } return true; @@ -953,8 +981,7 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects( return target_objects; } -LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const { const auto & current_pose = getEgoPose(); const auto & route_handler = getRouteHandler(); @@ -973,6 +1000,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( return {}; } + const auto & current_lanes = get_current_lanes(); + + if (current_lanes.empty()) { + return {}; + } + filterAheadTerminalObjects(objects, current_lanes); if (objects.objects.empty()) { @@ -983,6 +1016,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( std::vector current_lane_objects; std::vector other_lane_objects; + const auto & target_lanes = get_target_lanes(); + + if (target_lanes.empty()) { + return {}; + } + filterObjectsByLanelets( objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, other_lane_objects); @@ -1123,9 +1162,7 @@ void NormalLaneChange::filterObjectsByLanelets( }; // get backward lanes - const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = - utils::getPrecedingLanelets(*route_handler, target_lanes, current_pose, backward_length); + const auto & target_backward_lanes = common_data_ptr_->lanes_ptr->preceding_target; { lane_change_debug_.current_lanes = current_lanes; @@ -1142,12 +1179,7 @@ void NormalLaneChange::filterObjectsByLanelets( }); } - const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, - lane_change_parameters_->lane_expansion_right_offset); - - const auto lanes_polygon = - createLanesPolygon(current_lanes, expanded_target_lanes, target_backward_lanes); + const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); @@ -1167,7 +1199,7 @@ void NormalLaneChange::filterObjectsByLanelets( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }); - if (check_optional_polygon(object, lanes_polygon.target) && is_lateral_far) { + if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) { target_lane_objects.push_back(object); continue; } @@ -1177,7 +1209,7 @@ void NormalLaneChange::filterObjectsByLanelets( return isPolygonOverlapLanelet(object, target_backward_polygon); }; return std::any_of( - lanes_polygon.target_backward.begin(), lanes_polygon.target_backward.end(), + lanes_polygon.preceding_target.begin(), lanes_polygon.preceding_target.end(), check_backward_polygon); }); @@ -1367,14 +1399,14 @@ bool NormalLaneChange::getLaneChangePaths( const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + const auto & target_neighbor_preferred_lane_poly_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if (target_neighbor_preferred_lane_poly_2d.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } - const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto filtered_objects = filterObjects(); const auto target_objects = getTargetObjects(filtered_objects, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1445,7 +1477,7 @@ bool NormalLaneChange::getLaneChangePaths( RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); for (const auto & lateral_acc : sample_lat_acc) { - const auto debug_print = [&](const auto & s) { + const auto debug_print_lat = [&](const auto & s) { RCLCPP_DEBUG_STREAM( logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); @@ -1467,7 +1499,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - debug_print("Reject: length of lane changing path is longer than length to goal!!"); + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; } @@ -1486,7 +1518,7 @@ bool NormalLaneChange::getLaneChangePaths( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > s_goal) { - debug_print("Reject: length of lane changing path is longer than length to goal!!"); + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; } } @@ -1496,7 +1528,7 @@ bool NormalLaneChange::getLaneChangePaths( initial_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - debug_print("Reject: target segment is empty!! something wrong..."); + debug_print_lat("Reject: target segment is empty!! something wrong..."); continue; } @@ -1504,9 +1536,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment.points.back().point.pose.position.x, prepare_segment.points.back().point.pose.position.y); - const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( - target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + const auto target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); const auto is_valid_start_point = boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || @@ -1519,15 +1549,13 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_info.velocity = LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = lateral_acc; lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; if (!is_valid_start_point) { - debug_print( + debug_print_lat( "Reject: lane changing points are not inside of the target preferred lanes or its " "neighbors"); continue; @@ -1541,7 +1569,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - debug_print("Reject: target_lane_reference_path is empty!!"); + debug_print_lat("Reject: target_lane_reference_path is empty!!"); continue; } @@ -1549,16 +1577,16 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, target_segment, target_lane_reference_path, shift_length); const auto candidate_path = utils::lane_change::constructCandidatePath( - lane_change_info, prepare_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); + common_data_ptr_, lane_change_info, prepare_segment, target_segment, + target_lane_reference_path, sorted_lane_ids); if (!candidate_path) { - debug_print("Reject: failed to generate candidate path!!"); + debug_print_lat("Reject: failed to generate candidate path!!"); continue; } if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print("Reject: invalid candidate path!!"); + debug_print_lat("Reject: invalid candidate path!!"); continue; } @@ -1566,7 +1594,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_crosswalk && !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print("Reject: including crosswalk!!"); + debug_print_lat("Reject: including crosswalk!!"); continue; } RCLCPP_INFO_THROTTLE( @@ -1577,7 +1605,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_intersection && !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print("Reject: including intersection!!"); + debug_print_lat("Reject: including intersection!!"); continue; } RCLCPP_WARN_STREAM( @@ -1587,31 +1615,31 @@ bool NormalLaneChange::getLaneChangePaths( if ( lane_change_parameters_->regulate_on_traffic_light && !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { - debug_print("Reject: regulate on traffic light!!"); + debug_print_lat("Reject: regulate on traffic light!!"); continue; } if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - lane_change_info.current_lanes, candidate_path.value().path, planner_data_, + get_current_lanes(), candidate_path.value().path, planner_data_, lane_change_info.length.sum())) { - debug_print("Ego is stopping near traffic light. Do not allow lane change"); + debug_print_lat("Ego is stopping near traffic light. Do not allow lane change"); continue; } candidate_paths->push_back(*candidate_path); if ( - !is_stuck && utils::lane_change::passParkedObject( - route_handler, *candidate_path, filtered_objects.target_lane, - lane_change_buffer, is_goal_in_route, *lane_change_parameters_, - lane_change_debug_.collision_check_objects)) { - debug_print( + !is_stuck && + utils::lane_change::passParkedObject( + common_data_ptr_, *candidate_path, filtered_objects.target_lane, lane_change_buffer, + is_goal_in_route, lane_change_debug_.collision_check_objects)) { + debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); return false; } if (!check_safety) { - debug_print("ACCEPT!!!: it is valid (and safety check is skipped)."); + debug_print_lat("ACCEPT!!!: it is valid (and safety check is skipped)."); return false; } @@ -1619,11 +1647,11 @@ bool NormalLaneChange::getLaneChangePaths( *candidate_path, target_objects, rss_params, lane_change_debug_.collision_check_objects); if (is_safe) { - debug_print("ACCEPT!!!: it is valid and safe!"); + debug_print_lat("ACCEPT!!!: it is valid and safe!"); return true; } - debug_print("Reject: sampled path is not safe."); + debug_print_lat("Reject: sampled path is not safe."); } } } @@ -1661,7 +1689,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if (target_neighbor_preferred_lane_poly_2d.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return {}; @@ -1716,9 +1744,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::BasicPoint2d lc_start_point( lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); - const auto target_lane_polygon = - lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + const auto & target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); const auto is_valid_start_point = boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || @@ -1730,8 +1756,6 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.velocity = LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; lane_change_info.lane_changing_start = lane_changing_start_pose.value(); lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = max_lateral_acc; @@ -1771,8 +1795,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( - lane_change_info, reference_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); + common_data_ptr_, lane_change_info, reference_segment, target_segment, + target_lane_reference_path, sorted_lane_ids); return terminal_lane_change_path; } @@ -1780,10 +1804,14 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; - const auto & current_lanes = status_.current_lanes; - const auto & target_lanes = status_.target_lanes; + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + + if (current_lanes.empty() || target_lanes.empty()) { + return {true, true}; + } - const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto filtered_objects = filterObjects(); const auto target_objects = getTargetObjects(filtered_objects, current_lanes); CollisionCheckDebugMap debug_data; @@ -1832,13 +1860,14 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & route_handler = planner_data_->route_handler; const auto & dp = planner_data_->drivable_area_expansion_parameters; // check lane departure const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, utils::extendLanes(route_handler, status_.current_lanes), - utils::extendLanes(route_handler, status_.target_lanes)); + *route_handler, utils::extendLanes(route_handler, get_current_lanes()), + utils::extendLanes(route_handler, get_target_lanes())); const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); @@ -1869,9 +1898,10 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && + isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && isAbleToStopSafely() && is_object_coming_from_rear) { current_lane_change_state_ = LaneChangeStates::Stop; return true; @@ -1882,22 +1912,23 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) bool NormalLaneChange::calcAbortPath() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; - const auto reference_lanelets = selected_path.info.current_lanes; const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto direction = getDirection(); const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction); + route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); const auto & lane_changing_path = selected_path.path; + const auto & reference_lanelets = get_current_lanes(); const auto lane_changing_end_pose_idx = std::invoke([&]() { constexpr double s_start = 0.0; const double s_end = std::max( @@ -1986,7 +2017,7 @@ bool NormalLaneChange::calcAbortPath() auto reference_lane_segment = prev_module_output_.path; { // const auto terminal_path = - // calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + // calcTerminalLaneChangePath(reference_lanelets, get_target_lanes()); // if (terminal_path) { // reference_lane_segment = terminal_path->path; // } @@ -2025,6 +2056,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); PathSafetyStatus path_safety_status; if (collision_check_objects.empty()) { @@ -2050,12 +2082,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto debug_predicted_path = utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); - const auto current_lanes = getCurrentLanes(); - - const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - lane_change_path.info.target_lanes, direction_, - lane_change_parameters_->lane_expansion_left_offset, - lane_change_parameters_->lane_expansion_right_offset); + const auto & current_lanes_polygon = common_data_ptr_->lanes_polygon_ptr->current; + const auto & expanded_target_polygon = common_data_ptr_->lanes_polygon_ptr->expanded_target; constexpr double collision_check_yaw_diff_threshold{M_PI}; @@ -2076,10 +2104,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( continue; } - const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( - collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_current_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_lanes_polygon); const auto collision_in_target_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); if (!collision_in_current_lanes && !collision_in_target_lanes) { utils::path_safety_checker::updateCollisionCheckDebugMap( @@ -2108,6 +2136,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( bool NormalLaneChange::isVehicleStuck( const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // Ego is still moving, not in stuck if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); @@ -2163,6 +2192,7 @@ bool NormalLaneChange::isVehicleStuck( double NormalLaneChange::get_max_velocity_for_safety_check() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity; if (external_velocity_limit_ptr) { return std::min( @@ -2174,6 +2204,7 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { lane_change_debug_.is_stuck = false; return false; // can not check @@ -2208,6 +2239,7 @@ void NormalLaneChange::setStopPose(const Pose & stop_pose) void NormalLaneChange::updateStopTime() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_vel = getEgoVelocity(); if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index ad789975cbcd7..5f13aa3192113 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -330,13 +330,12 @@ bool isPathInLanelets( } std::optional constructCandidatePath( - const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, - const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids) { const auto & shift_line = lane_change_info.shift_line; - const auto & original_lanes = lane_change_info.current_lanes; - const auto & target_lanes = lane_change_info.target_lanes; const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; const auto lane_change_velocity = lane_change_info.velocity; @@ -393,9 +392,11 @@ std::optional constructCandidatePath( const bool enable_path_check_in_lanelet = false; // check candidate path is in lanelet + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; if ( enable_path_check_in_lanelet && - !isPathInLanelets(shifted_path.path, original_lanes, target_lanes)) { + !isPathInLanelets(shifted_path.path, current_lanes, target_lanes)) { return std::nullopt; } @@ -888,7 +889,7 @@ bool isParkedObject( const auto most_left_lanelet_candidates = route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = + const lanelet::Attribute most_left_sub_type = most_left_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_left_lanelet_candidates) { @@ -898,7 +899,7 @@ bool isParkedObject( } } bound = most_left_lanelet.leftBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { + if (most_left_sub_type.value() != "road_shoulder") { center_to_bound_buffer = object_check_min_road_shoulder_width; } } else { @@ -909,7 +910,7 @@ bool isParkedObject( most_right_road_lanelet.rightBound()); lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = + const lanelet::Attribute most_right_sub_type = most_right_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_right_lanelet_candidates) { @@ -919,7 +920,7 @@ bool isParkedObject( } } bound = most_right_lanelet.rightBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { + if (most_right_sub_type.value() != "road_shoulder") { center_to_bound_buffer = object_check_min_road_shoulder_width; } } @@ -968,17 +969,18 @@ bool isParkedObject( } bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, - CollisionCheckDebugMap & object_debug) + const bool is_goal_in_route, CollisionCheckDebugMap & object_debug) { + const auto route_handler = *common_data_ptr->route_handler_ptr; + const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = lane_change_parameters.object_shiftable_ratio_threshold; const auto & path = lane_change_path.path; - const auto & current_lanes = lane_change_path.info.current_lanes; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto current_lane_path = route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); @@ -1135,10 +1137,9 @@ ExtendedPredictedObject transform( } bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) + const std::vector & collided_polygons, + const std::optional & lanes_polygon) { - const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); - const auto is_in_lanes = [&](const auto & collided_polygon) { return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); }; @@ -1207,27 +1208,60 @@ double calcPhaseLength( return std::min(length_with_acceleration, length_with_max_velocity); } -LanesPolygon createLanesPolygon( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const std::vector & target_backward_lanes) +LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) { + const auto & lanes = common_data_ptr->lanes_ptr; LanesPolygon lanes_polygon; lanes_polygon.current = - utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); lanes_polygon.target = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); + + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset, + lc_param_ptr->lane_expansion_right_offset); + lanes_polygon.expanded_target = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); + + const auto & route_handler = *common_data_ptr->route_handler_ptr; + lanes_polygon.target_neighbor = + getTargetNeighborLanesPolygon(route_handler, lanes->current, common_data_ptr->lc_type); - for (const auto & target_backward_lane : target_backward_lanes) { - auto lane_polygon = utils::lane_change::createPolygon( - target_backward_lane, 0.0, std::numeric_limits::max()); + lanes_polygon.preceding_target.reserve(lanes->preceding_target.size()); + for (const auto & preceding_lane : lanes->preceding_target) { + auto lane_polygon = + utils::lane_change::createPolygon(preceding_lane, 0.0, std::numeric_limits::max()); if (lane_polygon) { - lanes_polygon.target_backward.push_back(*lane_polygon); + lanes_polygon.preceding_target.push_back(*lane_polygon); } } return lanes_polygon; } + +bool is_same_lane_with_prev_iteration( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) +{ + if (current_lanes.empty() || target_lanes.empty()) { + return false; + } + const auto & prev_current_lanes = common_data_ptr->lanes_ptr->current; + const auto & prev_target_lanes = common_data_ptr->lanes_ptr->target; + if (prev_current_lanes.empty() || prev_target_lanes.empty()) { + return false; + } + + if ( + (prev_current_lanes.front().id() != current_lanes.front().id()) || + (prev_current_lanes.back().id() != prev_current_lanes.back().id())) { + return false; + } + return (prev_target_lanes.front().id() == target_lanes.front().id()) && + (prev_target_lanes.back().id() == prev_target_lanes.back().id()); +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml index e5c3a76fd1c45..7d5249dea689f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: - max_iteration_num: 100 traffic_light_signal_timeout: 1.0 planning_hz: 10.0 backward_path_length: 5.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index a76284b70b7d8..0876d44b477a9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -21,6 +21,7 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include #include #include @@ -96,7 +97,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const size_t max_iteration_num); + explicit PlannerManager(rclcpp::Node & node); /** * @brief run all candidate and approved modules. @@ -111,6 +112,17 @@ class PlannerManager */ void launchScenePlugin(rclcpp::Node & node, const std::string & name); + /** + * @brief calculate max iteration numbers. + * Let N be the number of scene modules. The maximum number of iterations executed in a loop is N, + * but after that, if there are any modules that have succeeded or failed, the approve_modules of + * all modules are cleared, and the loop is executed for N-1 modules. As this process repeats, it + * becomes N + (N-1) + (N-2) + … + 1, therefore the maximum number of iterations is N(N+1)/2. + * @param number of scene module + * + */ + void calculateMaxIterationNum(const size_t scene_module_num); + /** * @brief unregister managers. * @param node. @@ -277,6 +289,7 @@ class PlannerManager const SceneModulePtr & module_ptr, const std::shared_ptr & planner_data, const BehaviorModuleOutput & previous_module_output) const { + universe_utils::ScopedTimeTrack st(module_ptr->name() + "=>run", *module_ptr->getTimeKeeper()); stop_watch_.tic(module_ptr->name()); module_ptr->setData(planner_data); @@ -396,11 +409,13 @@ class PlannerManager * @brief run all modules in approved_module_ptrs_ and get a planning result as * approved_modules_output. * @param planner data. + * @param deleted modules. * @return valid planning result. * @details in this function, expired modules (ModuleStatus::FAILURE or ModuleStatus::SUCCESS) are - * removed from approved_module_ptrs_. + * removed from approved_module_ptrs_ and added to deleted_modules. */ - BehaviorModuleOutput runApprovedModules(const std::shared_ptr & data); + BehaviorModuleOutput runApprovedModules( + const std::shared_ptr & data, std::vector & deleted_modules); /** * @brief select a module that should be execute at first. @@ -420,10 +435,12 @@ class PlannerManager /** * @brief get all modules that make execution request. * @param decided (=approved) path. + * @param deleted modules. * @return request modules. */ std::vector getRequestModules( - const BehaviorModuleOutput & previous_module_output) const; + const BehaviorModuleOutput & previous_module_output, + const std::vector & deleted_modules) const; /** * @brief checks whether a path of trajectory has forward driving direction diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 0e586211bc9ee..e92fe937708b8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -70,16 +70,18 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ - const auto & p = planner_data_->parameters; - planner_manager_ = std::make_shared(*this, p.max_iteration_num); + planner_manager_ = std::make_shared(*this); + size_t scene_module_num = 0; for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. if (name == "") { break; } planner_manager_->launchScenePlugin(*this, name); + scene_module_num++; } + planner_manager_->calculateMaxIterationNum(scene_module_num); for (const auto & manager : planner_manager_->getSceneModuleManagers()) { path_candidate_publishers_.emplace( @@ -147,7 +149,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; - p.max_iteration_num = declare_parameter("max_iteration_num"); p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); // vehicle info diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 05dcc2163d14a..5278ea8e76981 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -30,13 +30,12 @@ namespace autoware::behavior_path_planner { -PlannerManager::PlannerManager(rclcpp::Node & node, const size_t max_iteration_num) +PlannerManager::PlannerManager(rclcpp::Node & node) : plugin_loader_( "autoware_behavior_path_planner", "autoware::behavior_path_planner::SceneModuleManagerInterface"), logger_(node.get_logger().get_child("planner_manager")), - clock_(*node.get_clock()), - max_iteration_num_{max_iteration_num} + clock_(*node.get_clock()) { processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); @@ -66,6 +65,11 @@ void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & } } +void PlannerManager::calculateMaxIterationNum(const size_t scene_module_num) +{ + max_iteration_num_ = scene_module_num * (scene_module_num + 1) / 2; +} + void PlannerManager::removeScenePlugin(rclcpp::Node & node, const std::string & name) { auto it = std::remove_if(manager_ptrs_.begin(), manager_ptrs_.end(), [&](const auto plugin) { @@ -127,17 +131,19 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da "Ego is out of route, no module is running. Skip running scene modules."); return output; } + std::vector + deleted_modules; // store the scene modules deleted from approved modules for (size_t itr_num = 1;; ++itr_num) { /** * STEP1: get approved modules' output */ - auto approved_modules_output = runApprovedModules(data); + auto approved_modules_output = runApprovedModules(data, deleted_modules); /** * STEP2: check modules that need to be launched */ - const auto request_modules = getRequestModules(approved_modules_output); + const auto request_modules = getRequestModules(approved_modules_output, deleted_modules); /** * STEP3: if there is no module that need to be launched, return approved modules' output @@ -250,7 +256,8 @@ void PlannerManager::generateCombinedDrivableArea( } std::vector PlannerManager::getRequestModules( - const BehaviorModuleOutput & previous_module_output) const + const BehaviorModuleOutput & previous_module_output, + const std::vector & deleted_modules) const { if (previous_module_output.path.points.empty()) { RCLCPP_ERROR_STREAM( @@ -268,6 +275,18 @@ std::vector PlannerManager::getRequestModules( for (const auto & manager_ptr : manager_ptrs_) { stop_watch_.tic(manager_ptr->name()); + /** + * skip the module that is already deleted. + */ + { + const auto name = manager_ptr->name(); + const auto find_deleted_module = [&name](const auto & m) { return m->name() == name; }; + const auto itr = + std::find_if(deleted_modules.begin(), deleted_modules.end(), find_deleted_module); + if (itr != deleted_modules.end()) { + continue; + } + } /** * determine the execution capability of modules based on existing approved modules. @@ -655,7 +674,8 @@ std::pair PlannerManager::runRequestModule return std::make_pair(module_ptr, results.at(module_ptr->name())); } -BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr & data) +BehaviorModuleOutput PlannerManager::runApprovedModules( + const std::shared_ptr & data, std::vector & deleted_modules) { std::unordered_map results; BehaviorModuleOutput output = getReferencePath(data); @@ -771,6 +791,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() == ModuleStatus::FAILURE; }); + if (itr != approved_module_ptrs_.end()) { + deleted_modules.push_back(*itr); + } std::for_each(itr, approved_module_ptrs_.end(), [this](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); @@ -842,6 +865,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr #include #include +#include #include #include @@ -97,7 +98,8 @@ class SceneModuleInterface objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), steering_factor_interface_ptr_( - std::make_unique(&node, utils::convertToSnakeCase(name))) + std::make_unique(&node, utils::convertToSnakeCase(name))), + time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { uuid_map_.emplace(module_name, generateUUID()); @@ -244,6 +246,8 @@ class SceneModuleInterface previous_module_output_ = previous_module_output; } + std::shared_ptr getTimeKeeper() const { return time_keeper_; } + /** * @brief set planner data */ @@ -641,6 +645,8 @@ class SceneModuleInterface mutable MarkerArray debug_marker_; mutable MarkerArray drivable_lanes_marker_; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index fab9a6ba45113..c9adf4d60c21f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -85,6 +85,7 @@ class SceneModuleManagerInterface observer.lock()->setData(planner_data_); observer.lock()->setPreviousModuleOutput(previous_module_output); + observer.lock()->getTimeKeeper()->add_reporter(this->pub_processing_time_); observer.lock()->onEntry(); observers_.push_back(observer); @@ -318,6 +319,8 @@ class SceneModuleManagerInterface pub_debug_marker_ = node->create_publisher("~/debug/" + name_, 20); pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name_, 20); pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); + pub_processing_time_ = node->create_publisher( + "~/processing_time/" + name_, 20); } // misc @@ -338,6 +341,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; + rclcpp::Publisher::SharedPtr pub_processing_time_; + std::string name_; std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp index e5066932df88e..d34bfeb535f19 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp @@ -54,7 +54,7 @@ void modifyVelocityByDirection( const double acceleration); void updatePathProperty( - std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel); void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 4fd6fce325f49..f73f989174b54 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -56,18 +56,24 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; * * @param objects The predicted objects to filter. * @param lanelet + * @param yaw_threshold * @return result. */ -bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); +bool isCentroidWithinLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, + const double yaw_threshold); /** * @brief Filters objects based on object polygon overlapping with lanelet. * * @param objects The predicted objects to filter. * @param lanelet + * @param yaw_threshold * @return result. */ -bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); +bool isPolygonOverlapLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, + const double yaw_threshold); bool isPolygonOverlapLanelet( const PredictedObject & object, const autoware::universe_utils::Polygon2d & lanelet_polygon); @@ -168,7 +174,9 @@ void filterObjectsByClass( */ std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition); + const std::function & + condition, + const double yaw_threshold = M_PI); /** * @brief Separate the objects into two part based on whether the object is within lanelet. @@ -176,7 +184,9 @@ std::pair, std::vector> separateObjectIndicesByLanel */ std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition); + const std::function & + condition, + const double yaw_threshold = M_PI); /** * @brief Get the predicted path from an object. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp index 7ca11e7d7fd20..4301742a18fa7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -86,7 +86,7 @@ void modifyVelocityByDirection( } void updatePathProperty( - std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel) { // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 469be03eb905c..95a498b7b8475 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -57,15 +57,34 @@ bool is_within_circle( namespace autoware::behavior_path_planner::utils::path_safety_checker { -bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +bool isCentroidWithinLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold) { - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); + if ( + std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) > + yaw_threshold) { + return false; + } + + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pose.position)) + .basicPoint(), + lanelet.polygon2d().basicPolygon()); } -bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +bool isPolygonOverlapLanelet( + const PredictedObject & object, const lanelet::ConstLanelet & lanelet, const double yaw_threshold) { + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = lanelet::utils::getClosestCenterPose(lanelet, object_pose.position); + if ( + std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, object_pose)) > + yaw_threshold) { + return false; + } + const auto lanelet_polygon = utils::toPolygon2d(lanelet); return isPolygonOverlapLanelet(object, lanelet_polygon); } @@ -174,7 +193,9 @@ void filterObjectsByClass( std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition) + const std::function & + condition, + const double yaw_threshold) { if (target_lanelets.empty()) { return {}; @@ -184,7 +205,9 @@ std::pair, std::vector> separateObjectIndicesByLanel std::vector other_indices; for (size_t i = 0; i < objects.objects.size(); i++) { - const auto filter = [&](const auto & llt) { return condition(objects.objects.at(i), llt); }; + const auto filter = [&](const auto & llt) { + return condition(objects.objects.at(i), llt, yaw_threshold); + }; const auto found = std::find_if(target_lanelets.begin(), target_lanelets.end(), filter); if (found != target_lanelets.end()) { target_indices.push_back(i); @@ -198,13 +221,15 @@ std::pair, std::vector> separateObjectIndicesByLanel std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, - const std::function & condition) + const std::function & + condition, + const double yaw_threshold) { PredictedObjects target_objects; PredictedObjects other_objects; const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets, condition); + separateObjectIndicesByLanelets(objects, target_lanelets, condition, yaw_threshold); target_objects.objects.reserve(target_indices.size()); other_objects.objects.reserve(other_indices.size()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index f88efa49f10ff..8a6c899b4bd6d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -501,7 +501,7 @@ void PathShifter::removeBehindShiftLineAndSetBaseOffset(const size_t nearest_idx if (!removed_shift_lines.empty()) { const auto last_removed_sl = std::max_element( removed_shift_lines.begin(), removed_shift_lines.end(), - [](auto & a, auto & b) { return a.end_idx > b.end_idx; }); + [](const auto & a, const auto & b) { return a.end_idx > b.end_idx; }); new_base_offset = last_removed_sl->end_shift_length; } @@ -630,7 +630,7 @@ double PathShifter::getLastShiftLength() const const auto furthest = std::max_element( shift_lines_.begin(), shift_lines_.end(), - [](auto & a, auto & b) { return a.end_idx < b.end_idx; }); + [](const auto & a, const auto & b) { return a.end_idx < b.end_idx; }); return furthest->end_shift_length; } @@ -643,7 +643,7 @@ std::optional PathShifter::getLastShiftLine() const const auto furthest = std::max_element( shift_lines_.begin(), shift_lines_.end(), - [](auto & a, auto & b) { return a.end_idx > b.end_idx; }); + [](const auto & a, const auto & b) { return a.end_idx > b.end_idx; }); return *furthest; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index b7955da9a44f1..aa16f1bb23b79 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -1268,8 +1268,8 @@ lanelet::ConstLanelets getCurrentLanesFromPath( const auto & p = planner_data->parameters; std::set lane_ids; - for (const auto & p : path.points) { - for (const auto & id : p.lane_ids) { + for (const auto & point : path.points) { + for (const auto & id : point.lane_ids) { lane_ids.insert(id); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp index af8a538f894cc..b2b86433002cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp @@ -81,8 +81,6 @@ void SamplingPlannerModuleManager::updateModuleParams( auto & p = parameters_; - [[maybe_unused]] const std::string ns = name_ + "."; - { std::string ns{"constraints.hard"}; updateParam(parameters, ns + ".max_curvature", p->max_curvature); diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 526a761e6957a..3990a2fbf5475 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -553,8 +553,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan() prepareSamplingParameters(future_state, reference_spline, *internal_params_); auto extension_frenet_paths = autoware::frenet_planner::generatePaths( reference_spline, frenet_reuse_state, extension_sampling_parameters); - for (auto & p : extension_frenet_paths) { - if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p)); + for (auto & path : extension_frenet_paths) { + if (!path.points.empty()) frenet_paths.push_back(reused_path.extend(path)); } } } else { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 7b30745057743..03b6e13cb2d3e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -74,8 +74,9 @@ class PullOutPlannerBase *dynamic_objects, parameters_.th_moving_object_velocity); auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + stop_objects, pull_out_lanes, + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); utils::path_safety_checker::filterObjectsByClass( pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 7c78f62192105..f1e0f8abb2395 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -423,8 +423,17 @@ double ShiftPullOut::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : - autoware::motion_utils::calcCurvatureAndArcLength(path.points)) { + const auto curvatures_and_segment_lengths = + autoware::motion_utils::calcCurvatureAndSegmentLength(path.points); + for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) { + const auto & [k, segment_length_pair] = curvatures_and_segment_lengths.at(i); + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvatures_and_segment_lengths.size() - 1 + ? segment_length_pair.first + segment_length_pair.second + : segment_length_pair.first; + // after shifted segment length const double after_segment_length = k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 00b6649851f87..9b94d3505c405 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1217,8 +1217,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( // filter for objects located in pull out lanes and moving at a speed below the threshold auto [stop_objects_in_pull_out_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) { - return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + stop_objects, pull_out_lanes, + [](const auto & obj, const auto & lane, const auto yaw_threshold) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold); }); const auto path = planner_data_->route_handler->getCenterLinePath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index 40d36898b2309..036692c1c6b89 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -401,6 +401,15 @@ else (\n no) #00FFB1 :IGNORE; stop endif +if(Is UNKNOWN objects?) then (yes) +if(isSatisfiedWithUnknownTypeObjectCodition) then (yes) +#FF006C :AVOID; +stop +else (\n no) +#00FFB1 :IGNORE; +stop +endif +else (\n no) if(Is vehicle type?) then (yes) if(isSatisfiedWithVehicleCodition()) then (yes) else (\n no) @@ -618,6 +627,18 @@ title Filter vehicle which is obviously an avoidance target start partition isObviousAvoidanceTarget() { +if(Is object within freespace?) then (yes) +if(Is object on ego lane?) then (no) +if(Is object stopping longer than threshold?) then (yes) +#FF006C :return true; +stop +else (\n no) +endif +else (\n yes) +endif +else (\n no) +endif + if(Is object within intersection?) then (yes) else (\n no) if(Is object parallel to ego lane? AND Is object a parked vehicle?) then (yes) @@ -659,6 +680,27 @@ if(isWithinCrosswalk()) then (yes) stop else (\n no) endif + +if(is object within intersection?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif + +if(is object on right side of the ego path?) then (yes) +if(are there adjacent lanes on right side of ego lane?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif +else (\n no) +if(are there adjacent lanes on left side of ego lane?) then (yes) +#00FFB1 :return false; +stop +else (\n no) +endif +endif + #FF006C :return true; stop } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 3087ccc93934b..0e60e3216361d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -138,8 +138,11 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: - enable: true # [-] - closest_distance_to_wait_and_see: 10.0 # [m] + # policy for ego behavior for ambiguous vehicle. + # "auto" : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically. + # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode. + # "ignore" : never avoid it. + policy: "auto" # [-] condition: th_stopped_time: 3.0 # [s] th_moving_distance: 1.0 # [m] @@ -149,11 +152,18 @@ crosswalk: front_distance: 30.0 # [m] behind_distance: 30.0 # [m] + wait_and_see: + target_behaviors: ["MERGING", "DEVIATING"] # [-] + th_closest_distance: 10.0 # [m] # params for filtering objects that are in intersection intersection: yaw_deviation: 0.349 # [rad] (default 20.0deg) + freespace: + condition: + th_stopped_time: 5.0 # [-] + # For safety check safety_check: # safety check target type diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index d02b39047e71c..4e14dc4886768 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -108,7 +108,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_avoidance_for_ambiguous_vehicle{false}; + std::string policy_ambiguous_vehicle{"ignore"}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -191,8 +191,12 @@ struct AvoidanceParameters // minimum road shoulder width. maybe 0.5 [m] double object_check_min_road_shoulder_width{0.0}; + // time threshold for vehicle in freespace. + double freespace_condition_th_stopped_time{0.0}; + // force avoidance - double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + std::vector wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"}; + double wait_and_see_th_closest_distance{0.0}; double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; @@ -352,6 +356,10 @@ struct ObjectData // avoidance target { } + Pose getPose() const { return object.kinematics.initial_pose_with_covariance.pose; } + + Point getPosition() const { return object.kinematics.initial_pose_with_covariance.pose.position; } + PredictedObject object; // object behavior. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp index 316501fbbd37f..b205838896038 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -47,6 +47,11 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray( const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); +MarkerArray createAmbiguousObjectsMarkerArray( + const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy); + +MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data); + MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 8c46affbc64e3..bfeb942c82be3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -325,10 +326,27 @@ class AvoidanceHelper const auto object = objects.front(); + // if the object is NOT ambiguous, this module doesn't wait operator approval if RTC is running + // as AUTO mode. if (!object.is_ambiguous) { return true; } + // check only front objects. + if (object.longitudinal < 0.0) { + return true; + } + + // if the policy is "manual", this module generates candidate path and waits approval. + if (parameters_->policy_ambiguous_vehicle == "manual") { + return false; + } + + // don't delay avoidance start position if it's not MERGING or DEVIATING vehicle. + if (!isWaitAndSeeTarget(object)) { + return true; + } + if (!object.avoid_margin.has_value()) { return true; } @@ -341,9 +359,32 @@ class AvoidanceHelper const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); - return object.longitudinal < - prepare_distance + constant_distance + avoidance_distance + - parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + + parameters_->wait_and_see_th_closest_distance; + } + + bool isWaitAndSeeTarget(const ObjectData & object) const + { + const auto & behaviors = parameters_->wait_and_see_target_behaviors; + if (object.behavior == ObjectData::Behavior::MERGING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "MERGING"; + }); + } + + if (object.behavior == ObjectData::Behavior::DEVIATING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "DEVIATING"; + }); + } + + if (object.behavior == ObjectData::Behavior::NONE) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "NONE"; + }); + } + + return false; } static bool isAbsolutelyNotAvoidable(const ObjectData & object) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 84cf7c4e33d26..56ac3d7f4f1bb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -137,9 +137,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); - p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = @@ -152,6 +154,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); } + { + const std::string ns = "avoidance.target_filtering.freespace."; + p.freespace_condition_th_stopped_time = + getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); + } + { const std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 635edb7c84f40..34d06a46d9ac8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -266,6 +266,12 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface */ void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; + /** + * @brief fill additional data which are necessary to plan avoidance path/velocity. + * @param avoidance target objects. + */ + void fillAvoidanceTargetData(ObjectDataArray & objects) const; + /** * @brief fill candidate shift lines. * @param avoidance data. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp index 3797fe0d55147..a3b1b7e10b885 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -118,15 +118,12 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); -void updateRegisteredObject( - ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, - const std::shared_ptr & parameters); - void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data); -void compensateDetectionLost( - const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, - ObjectDataArray & other_objects); +void compensateLostTargetObjects( + ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); void filterTargetObjects( ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index db3215fa8d238..38a91ac39525b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -716,9 +716,10 @@ "avoidance_for_ambiguous_vehicle": { "type": "object", "properties": { - "enable": { - "type": "boolean", - "description": "Enable avoidance maneuver for ambiguous vehicles.", + "policy": { + "type": "string", + "enum": ["auto", "manual", "ignore"], + "description": "Ego behavior policy for ambiguous vehicle.", "default": "true" }, "closest_distance_to_wait_and_see": { @@ -778,14 +779,26 @@ }, "required": ["traffic_light", "crosswalk"], "additionalProperties": false + }, + "wait_and_see": { + "type": "object", + "properties": { + "target_behaviors": { + "type": "array", + "default": ["MERGING", "DEVIATING"], + "description": "This module doesn't avoid these behaviors vehicle until it gets closer than threshold." + }, + "th_closest_distance": { + "type": "number", + "default": 10.0, + "description": "Threshold to check whether the ego gets close enough the ambiguous vehicle." + } + }, + "required": ["target_behaviors", "th_closest_distance"], + "additionalProperties": false } }, - "required": [ - "enable", - "closest_distance_to_wait_and_see", - "condition", - "ignore_area" - ], + "required": ["policy", "condition", "ignore_area", "wait_and_see"], "additionalProperties": false }, "intersection": { @@ -799,6 +812,25 @@ }, "required": ["yaw_deviation"], "additionalProperties": false + }, + "freespace": { + "type": "object", + "properties": { + "condition": { + "type": "object", + "properties": { + "th_stopped_time": { + "type": "number", + "description": "This module delays avoidance maneuver to see vehicle behavior in freespace.", + "default": 5.0 + } + }, + "required": ["th_stopped_time"], + "additionalProperties": false + } + }, + "required": ["condition"], + "additionalProperties": false } }, "required": [ @@ -810,7 +842,8 @@ "merging_vehicle", "parked_vehicle", "avoidance_for_ambiguous_vehicle", - "intersection" + "intersection", + "freespace" ], "additionalProperties": false }, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 9c771224e89eb..fb4e5c7b9a25e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -64,7 +64,7 @@ MarkerArray createObjectsCubeMarkerArray( } marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); msg.markers.push_back(marker); } @@ -80,10 +80,8 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std: "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - for (const auto & p : object.envelope_poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); + marker.points.push_back(createPoint(p.x(), p.y(), object.getPosition().z)); } marker.points.push_back(marker.points.front()); @@ -142,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray( for (const auto & object : objects) { if (verbose) { marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" @@ -162,7 +160,7 @@ MarkerArray createObjectInfoMarkerArray( { marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); marker.pose.position.z += 2.0; std::ostringstream string_stream; string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : ""); @@ -458,7 +456,7 @@ MarkerArray createOtherObjectsMarkerArray( appendMarkerArray( createObjectsCubeMarkerArray( filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(0.0, 1.0, 0.0, 0.8)), + createMarkerColor(0.5, 0.5, 0.5, 0.8)), &msg); appendMarkerArray( createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg); @@ -468,6 +466,94 @@ MarkerArray createOtherObjectsMarkerArray( return msg; } +MarkerArray createAmbiguousObjectsMarkerArray( + const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy) +{ + MarkerArray msg; + + if (policy != "manual") { + return msg; + } + + for (const auto & object : objects) { + if (!object.is_ambiguous || !object.is_avoidable) { + continue; + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target", 0L, Marker::ARROW, + createMarkerScale(0.5, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + + Point src, dst; + src = object.getPosition(); + src.z += 4.0; + dst = object.getPosition(); + dst.z += 2.0; + + marker.points.push_back(src); + marker.points.push_back(dst); + marker.id = uuidToInt32(object.object.object_id); + + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target_text", 0L, + Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), + createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.getPose(); + marker.pose.position.z += 4.5; + std::ostringstream string_stream; + string_stream << "SHOULD AVOID?"; + marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + marker.scale = createMarkerScale(0.8, 0.8, 0.8); + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "request_text", 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.id = uuidToInt32(object.object.object_id); + marker.pose = ego_pose; + marker.pose.position.z += 2.0; + std::ostringstream string_stream; + string_stream << "SYSTEM REQUESTS OPERATOR SUPPORT."; + marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); + marker.scale = createMarkerScale(0.8, 0.8, 0.8); + msg.markers.push_back(marker); + } + + return msg; + } + + return msg; +} + +MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data) +{ + MarkerArray msg; + + if (!data.stop_target_object.has_value()) { + return msg; + } + + appendMarkerArray( + createObjectsCubeMarkerArray( + {data.stop_target_object.value()}, "stop_target", createMarkerScale(3.4, 1.9, 1.9), + createMarkerColor(1.0, 0.0, 0.42, 0.5)), + &msg); + + return msg; +} + MarkerArray createDrivableBounds( const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g, const float & b) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index 2d02f33e19870..5655696ff086d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -132,10 +132,9 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( { const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; - updateParam(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); + updateParam(parameters, ns + "policy", p->policy_ambiguous_vehicle); updateParam( - parameters, ns + "closest_distance_to_wait_and_see", - p->closest_distance_to_wait_and_see_for_ambiguous_vehicle); + parameters, ns + "wait_and_see.th_closest_distance", p->wait_and_see_th_closest_distance); updateParam( parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle); updateParam( @@ -151,6 +150,12 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( p->object_ignore_section_crosswalk_behind_distance); } + { + const std::string ns = "avoidance.target_filtering.freespace."; + updateParam( + parameters, ns + "condition.th_stopped_time", p->freespace_condition_th_stopped_time); + } + { const std::string ns = "avoidance.target_filtering.intersection."; updateParam(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index b0e7d68c75d44..b5b9378618a60 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -24,6 +24,7 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include #include #include @@ -194,6 +195,7 @@ bool StaticObstacleAvoidanceModule::canTransitSuccessState() void StaticObstacleAvoidanceModule::fillFundamentalData( AvoidancePlanningData & data, DebugData & debug) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference pose data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_->getPreviousSplineShiftPath()); @@ -286,16 +288,22 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); - // target objects for avoidance + // filter only for the latest detected objects. fillAvoidanceTargetObjects(data, debug); - // lost object compensation - utils::static_obstacle_avoidance::updateRegisteredObject( - registered_objects_, data.target_objects, parameters_); - utils::static_obstacle_avoidance::compensateDetectionLost( - registered_objects_, data.target_objects, data.other_objects); + // compensate lost object which was avoidance target. if the time hasn't passed more than + // threshold since perception module lost the target yet, this module keeps it as avoidance + // target. + utils::static_obstacle_avoidance::compensateLostTargetObjects( + registered_objects_, data, clock_->now(), planner_data_, parameters_); + + // once an object filtered for boundary clipping, this module keeps the information until the end + // of execution. utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data); + // calculate various data for each target objects. + fillAvoidanceTargetData(data.target_objects); + // sort object order by longitudinal distance std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) { return a.longitudinal < b.longitudinal; @@ -308,8 +316,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - using utils::static_obstacle_avoidance::fillAvoidanceNecessity; - using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::filterTargetObjects; using utils::static_obstacle_avoidance::separateObjectsByPath; using utils::static_obstacle_avoidance::updateRoadShoulderDistance; @@ -345,15 +352,6 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); updateRoadShoulderDistance(data, planner_data_, parameters_); - // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); - std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { - fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); - o.to_stop_line = calcDistanceToStopLine(o); - fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); - }); - // debug { std::vector debug_info_array; @@ -371,9 +369,26 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( } } +void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const +{ + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + using utils::static_obstacle_avoidance::fillAvoidanceNecessity; + using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + + // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); + std::for_each(objects.begin(), objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); + o.to_stop_line = calcDistanceToStopLine(o); + fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); + }); +} + ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using boost::geometry::return_centroid; const auto & path_points = data.reference_path.points; @@ -415,6 +430,7 @@ ObjectData StaticObstacleAvoidanceModule::createObjectData( bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // transit yield maneuver only when the avoidance maneuver is not initiated. if (helper_->isShifted()) { RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated."); @@ -468,6 +484,7 @@ bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData void StaticObstacleAvoidanceModule::fillShiftLine( AvoidancePlanningData & data, DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto path_shifter = path_shifter_; /** @@ -520,6 +537,7 @@ void StaticObstacleAvoidanceModule::fillShiftLine( void StaticObstacleAvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); data.state = getCurrentModuleState(data); /** @@ -614,6 +632,7 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( void StaticObstacleAvoidanceModule::fillDebugData( const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!data.stop_target_object) { return; } @@ -694,6 +713,7 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( bool StaticObstacleAvoidanceModule::isSafePath( ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data_->parameters; if (!parameters_->enable_safety_check) { @@ -802,6 +822,7 @@ bool StaticObstacleAvoidanceModule::isSafePath( PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( const PathWithLaneId & original_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto previous_path = helper_->getPreviousReferencePath(); const auto longest_dist_to_shift_point = [&]() { @@ -864,6 +885,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; resetPathCandidate(); @@ -1021,6 +1043,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; CandidateOutput output; @@ -1060,6 +1083,7 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const BehaviorModuleOutput StaticObstacleAvoidanceModule::planWaitingApproval() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); BehaviorModuleOutput out = plan(); if (path_shifter_.getShiftLines().empty()) { @@ -1116,6 +1140,7 @@ void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shi void StaticObstacleAvoidanceModule::addNewShiftLines( PathShifter & path_shifter, const AvoidLineArray & new_shift_lines) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); ShiftLineArray future = utils::static_obstacle_avoidance::toShiftLineArray(new_shift_lines); size_t min_start_idx = std::numeric_limits::max(); @@ -1182,6 +1207,7 @@ void StaticObstacleAvoidanceModule::addNewShiftLines( bool StaticObstacleAvoidanceModule::isValidShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (shift_lines.empty()) { return true; } @@ -1257,6 +1283,7 @@ bool StaticObstacleAvoidanceModule::isValidShiftLine( void StaticObstacleAvoidanceModule::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::toShiftedPath; helper_->setData(planner_data_); @@ -1341,6 +1368,7 @@ void StaticObstacleAvoidanceModule::initRTCStatus() void StaticObstacleAvoidanceModule::updateRTCData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; updateRegisteredRTCStatus(helper_->getPreviousSplineShiftPath().path); @@ -1376,16 +1404,25 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray; + using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); appendMarkerArray( createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); + appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_); + appendMarkerArray( + createAmbiguousObjectsMarkerArray( + data.target_objects, getEgoPose(), parameters_->policy_ambiguous_vehicle), + &info_marker_); } void StaticObstacleAvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); debug_marker_.markers.clear(); debug_marker_ = utils::static_obstacle_avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); @@ -1410,6 +1447,7 @@ void StaticObstacleAvoidanceModule::updateAvoidanceDebugData( double StaticObstacleAvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = parameters_; const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -1451,6 +1489,7 @@ double StaticObstacleAvoidanceModule::calcDistanceToStopLine(const ObjectData & void StaticObstacleAvoidanceModule::insertReturnDeadLine( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; if (data.to_return_point > planner_data_->parameters.forward_path_length) { @@ -1528,6 +1567,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( void StaticObstacleAvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // If avoidance path is NOT valid, don't insert any stop points. @@ -1576,6 +1616,7 @@ void StaticObstacleAvoidanceModule::insertWaitPoint( void StaticObstacleAvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; if (data.safe) { @@ -1623,6 +1664,7 @@ void StaticObstacleAvoidanceModule::insertStopPoint( void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // If avoidance path is NOT safe, don't insert any slow down sections. @@ -1735,6 +1777,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; // do nothing if no shift line is approved. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index a171ae9161041..c3a67eb074d73 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -165,9 +165,9 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // the avoidance path is already approved - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - const auto is_approved = (helper_->getShift(object_pos) > 0.0 && is_object_on_right) || - (helper_->getShift(object_pos) < 0.0 && !is_object_on_right); + const auto is_approved = + (helper_->getShift(object.getPosition()) > 0.0 && is_object_on_right) || + (helper_->getShift(object.getPosition()) < 0.0 && !is_object_on_right); if (is_approved) { return std::make_pair(desire_shift_length, avoidance_distance); } @@ -363,9 +363,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( if (is_return_shift_to_goal) { return true; } - const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; const bool has_object_near_goal = - autoware::universe_utils::calcDistance2d(goal_pose.position, object_pos) < + autoware::universe_utils::calcDistance2d(goal_pose.position, o.getPosition()) < parameters_->object_check_goal_distance; return has_object_near_goal; }(); @@ -1027,8 +1026,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const auto has_object_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { return autoware::universe_utils::calcDistance2d( - data_->route_handler->getGoalPose().position, - o.object.kinematics.initial_pose_with_covariance.pose.position) < + data_->route_handler->getGoalPose().position, o.getPosition()) < parameters_->object_check_goal_distance; }); if (has_object_near_goal) { @@ -1097,9 +1095,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const bool has_last_shift_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return autoware::universe_utils::calcDistance2d( - last_sl.end.position, - o.object.kinematics.initial_pose_with_covariance.pose.position) < + return autoware::universe_utils::calcDistance2d(last_sl.end.position, o.getPosition()) < parameters_->object_check_goal_distance; }); if (has_last_shift_near_goal) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 03fe48c6ab441..ca1ee624a54d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -293,8 +293,7 @@ bool isWithinCrosswalk( { using Point = boost::geometry::model::d2::point_xy; - const auto & p = object.object.kinematics.initial_pose_with_covariance.pose.position; - const Point p_object{p.x, p.y}; + const Point p_object{object.getPosition().x, object.getPosition().y}; // get conflicting crosswalk crosswalk constexpr int PEDESTRIAN_GRAPH_ID = 1; @@ -335,14 +334,42 @@ bool isWithinIntersection( route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); return boost::geometry::within( - object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lanelet::utils::to2D(polygon.basicPolygon())); +} + +bool isWithinFreespace( + const ObjectData & object, const std::shared_ptr & route_handler) +{ + auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr()); + if (polygons.empty()) { + return false; + } + + std::sort(polygons.begin(), polygons.end(), [&object](const auto & a, const auto & b) { + const double a_distance = boost::geometry::distance( + lanelet::utils::to2D(a).basicPolygon(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint()); + const double b_distance = boost::geometry::distance( + lanelet::utils::to2D(b).basicPolygon(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint()); + return a_distance < b_distance; + }); + + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lanelet::utils::to2D(polygons.front().basicPolygon())); } bool isOnEgoLane(const ObjectData & object, const std::shared_ptr & route_handler) { - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), object.overhang_lanelet.polygon2d().basicPolygon())) { return true; } @@ -351,7 +378,8 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr lanelet::ConstLanelets prev_lanelet; if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) { if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), prev_lanelet.front().polygon2d().basicPolygon())) { return true; } @@ -361,10 +389,20 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr lanelet::ConstLanelet next_lanelet; if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), next_lanelet.polygon2d().basicPolygon())) { return true; } + } else { + for (const auto & lane : route_handler->getNextLanelets(object.overhang_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lane.polygon2d().basicPolygon())) { + return true; + } + } } return false; @@ -372,20 +410,18 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr bool isParallelToEgoLane(const ObjectData & object, const double threshold) { - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto closest_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose)); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); + const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object.getPose())); return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; } bool isMergingToEgoLane(const ObjectData & object) { - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto closest_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); + const auto yaw_deviation = calcYawDeviation(closest_pose, object.getPose()); if (isOnRight(object)) { if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) { @@ -422,9 +458,8 @@ bool isParkedVehicle( return false; } - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; const auto centerline_pos = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position; + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()).position; bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { @@ -442,7 +477,8 @@ bool isParkedVehicle( return same_direction_lane; } - return static_cast(opposite_lanes.front().invert()); + return static_cast( + route_handler->getMostRightLanelet(opposite_lanes.front()).invert()); }(); const auto center_to_left_boundary = distance2d( @@ -457,7 +493,7 @@ bool isParkedVehicle( if (sub_type == "road_shoulder") { // assuming it's parked vehicle if its CoG is within road shoulder lanelet. if (boost::geometry::within( - to2D(toLaneletPoint(object_pos)).basicPoint(), + to2D(toLaneletPoint(object.getPosition())).basicPoint(), most_left_lanelet.polygon2d().basicPolygon())) { return true; } @@ -468,7 +504,7 @@ bool isParkedVehicle( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pos)).basicPoint()); + to2D(toLaneletPoint(object.getPosition())).basicPoint()); object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -490,7 +526,8 @@ bool isParkedVehicle( return same_direction_lane; } - return static_cast(opposite_lanes.front().invert()); + return static_cast( + route_handler->getMostLeftLanelet(opposite_lanes.front()).invert()); }(); const auto center_to_right_boundary = distance2d( @@ -505,7 +542,7 @@ bool isParkedVehicle( if (sub_type == "road_shoulder") { // assuming it's parked vehicle if its CoG is within road shoulder lanelet. if (boost::geometry::within( - to2D(toLaneletPoint(object_pos)).basicPoint(), + to2D(toLaneletPoint(object.getPosition())).basicPoint(), most_right_lanelet.polygon2d().basicPolygon())) { return true; } @@ -516,7 +553,7 @@ bool isParkedVehicle( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pos)).basicPoint()); + to2D(toLaneletPoint(object.getPosition())).basicPoint()); object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = @@ -527,9 +564,8 @@ bool isParkedVehicle( return false; } - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { return false; } @@ -544,13 +580,13 @@ bool isCloseToStopFactor( { const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle bool is_close_to_stop_factor = false; // check traffic light - const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); + const auto to_traffic_light = + getDistanceToNextTrafficLight(object.getPose(), data.extend_lanelets); { is_close_to_stop_factor = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; @@ -698,6 +734,14 @@ bool isObviousAvoidanceTarget( [[maybe_unused]] const std::shared_ptr & planner_data, [[maybe_unused]] const std::shared_ptr & parameters) { + if (isWithinFreespace(object, planner_data->route_handler)) { + if (!object.is_on_ego_lane) { + if (object.stop_time > parameters->freespace_condition_th_stopped_time) { + return true; + } + } + } + if (!object.is_within_intersection) { if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is obvious parked vehicle."); @@ -792,14 +836,52 @@ bool isSatisfiedWithNonVehicleCondition( } // Object is on center line -> ignore. - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { object.info = ObjectInfo::TOO_NEAR_TO_CENTERLINE; return false; } + if (object.is_within_intersection) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "object is within intersection. don't have to avoid it."); + return false; + } + + const auto right_lane = + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, true); + if (right_lane.has_value() && isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto left_lane = + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true); + if (left_lane.has_value() && !isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto right_opposite_lanes = + planner_data->route_handler->getRightOppositeLanelets(object.overhang_lanelet); + if (!right_opposite_lanes.empty() && isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + + const auto left_opposite_lanes = + planner_data->route_handler->getLeftOppositeLanelets(object.overhang_lanelet); + if (!left_opposite_lanes.empty() && !isOnRight(object)) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return false; + } + return true; } @@ -832,7 +914,7 @@ bool isSatisfiedWithVehicleCondition( // from here, filtering for ambiguous vehicle. - if (!parameters->enable_avoidance_for_ambiguous_vehicle) { + if (parameters->policy_ambiguous_vehicle == "ignore") { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; return false; } @@ -841,15 +923,16 @@ bool isSatisfiedWithVehicleCondition( object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; if (!stop_time_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } - const auto & current_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto is_moving_distance_longer_than_threshold = - calcDistance2d(object.init_pose, current_pose) > + calcDistance2d(object.init_pose, object.getPose()) > parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -860,22 +943,9 @@ bool isSatisfiedWithVehicleCondition( return true; } } else { - if (object.behavior == ObjectData::Behavior::MERGING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::DEVIATING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::NONE) { - object.is_ambiguous = false; - return true; - } + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = true; + return true; } object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; @@ -944,9 +1014,8 @@ double getRoadShoulderDistance( using autoware::universe_utils::Point2d; using lanelet::utils::to2D; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - autoware::motion_utils::findNearestIndex(data.reference_path.points, object_pose.position); + autoware::motion_utils::findNearestIndex(data.reference_path.points, object.getPosition()); const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; const auto rh = planner_data->route_handler; @@ -957,7 +1026,7 @@ double getRoadShoulderDistance( std::vector> intersects; for (const auto & p1 : object.overhang_points) { const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); const auto p_tmp = geometry_msgs::build().position(p1.second).orientation(centerline_pose.orientation); @@ -1326,8 +1395,7 @@ std::vector generateObstaclePolygonsForDrivableArea( object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = autoware::universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - obstacles_for_drivable_area.push_back( - {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); + obstacles_for_drivable_area.push_back({object.getPose(), obj_poly, !isOnRight(object)}); } return obstacles_for_drivable_area; } @@ -1495,7 +1563,7 @@ void fillObjectMovingTime( object_data.last_stop = now; object_data.move_time = 0.0; if (is_new_object) { - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); object_data.stop_time = 0.0; object_data.last_move = now; stopped_objects.push_back(object_data); @@ -1510,7 +1578,7 @@ void fillObjectMovingTime( } if (is_new_object) { - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); object_data.move_time = std::numeric_limits::infinity(); object_data.stop_time = 0.0; object_data.last_move = now; @@ -1520,7 +1588,7 @@ void fillObjectMovingTime( object_data.last_stop = same_id_obj->last_stop; object_data.move_time = (now - same_id_obj->last_stop).seconds(); object_data.stop_time = 0.0; - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); if (object_data.move_time > object_parameter.moving_time_threshold) { stopped_objects.erase(same_id_obj); @@ -1600,31 +1668,39 @@ void fillObjectStoppableJudge( object_data.is_stoppable = same_id_obj->is_stoppable; } -void updateRegisteredObject( - ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, +void compensateLostTargetObjects( + ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now, + const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - const auto updateIfDetectedNow = [&now_objects](auto & registered_object) { - const auto & n = now_objects; - const auto r_id = registered_object.object.object_id; - const auto same_id_obj = std::find_if( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + const auto include = [](const auto & objects, const auto & search_id) { + return std::all_of(objects.begin(), objects.end(), [&search_id](const auto & o) { + return o.object.object_id != search_id; + }); + }; + + // STEP.1 UPDATE STORED OBJECTS. + const auto match = [&data](auto & object) { + const auto & search_id = object.object.object_id; + const auto same_id_object = std::find_if( + data.target_objects.begin(), data.target_objects.end(), + [&search_id](const auto & o) { return o.object.object_id == search_id; }); // same id object is detected. update registered. - if (same_id_obj != n.end()) { - registered_object = *same_id_obj; + if (same_id_object != data.target_objects.end()) { + object = *same_id_object; return true; } - constexpr auto POS_THR = 1.5; - const auto r_pos = registered_object.object.kinematics.initial_pose_with_covariance.pose; - const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { - return calcDistance2d(r_pos, o.object.kinematics.initial_pose_with_covariance.pose) < POS_THR; - }); + const auto similar_pos_obj = std::find_if( + data.target_objects.begin(), data.target_objects.end(), [&object](const auto & o) { + constexpr auto POS_THR = 1.5; + return calcDistance2d(object.getPose(), o.getPose()) < POS_THR; + }); // same id object is not detected, but object is found around registered. update registered. - if (similar_pos_obj != n.end()) { - registered_object = *similar_pos_obj; + if (similar_pos_obj != data.target_objects.end()) { + object = *similar_pos_obj; return true; } @@ -1632,61 +1708,54 @@ void updateRegisteredObject( return false; }; - const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); + // STEP1-1: REMOVE EXPIRED OBJECTS. + const auto itr = std::remove_if( + stored_objects.begin(), stored_objects.end(), [&now, &match, ¶meters](auto & o) { + if (!match(o)) { + o.lost_time = (now - o.last_seen).seconds(); + } else { + o.last_seen = now; + o.lost_time = 0.0; + } - // -- check registered_objects, remove if lost_count exceeds limit. -- - for (int i = static_cast(registered_objects.size()) - 1; i >= 0; --i) { - auto & r = registered_objects.at(i); + return o.lost_time > parameters->object_last_seen_threshold; + }); - // registered object is not detected this time. lost count up. - if (!updateIfDetectedNow(r)) { - r.lost_time = (now - r.last_seen).seconds(); - } else { - r.last_seen = now; - r.lost_time = 0.0; - } + stored_objects.erase(itr, stored_objects.end()); - // lost count exceeds threshold. remove object from register. - if (r.lost_time > parameters->object_last_seen_threshold) { - registered_objects.erase(registered_objects.begin() + i); + // STEP1-2: UPDATE STORED OBJECTS IF THERE ARE NEW OBJECTS. + for (const auto & current_object : data.target_objects) { + if (!include(stored_objects, current_object.object.object_id)) { + stored_objects.push_back(current_object); } } - const auto isAlreadyRegistered = [&](const auto & n_id) { - const auto & r = registered_objects; + // STEP2: COMPENSATE CURRENT TARGET OBJECTS + const auto is_detected = [&](const auto & object_id) { return std::any_of( - r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; }); + data.target_objects.begin(), data.target_objects.end(), + [&object_id](const auto & o) { return o.object.object_id == object_id; }); }; - // -- check now_objects, add it if it has new object id -- - for (const auto & now_obj : now_objects) { - if (!isAlreadyRegistered(now_obj.object.object_id)) { - registered_objects.push_back(now_obj); - } - } -} - -void compensateDetectionLost( - const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, - ObjectDataArray & other_objects) -{ - const auto isDetectedNow = [&](const auto & r_id) { - const auto & n = now_objects; + const auto is_ignored = [&](const auto & object_id) { return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + data.other_objects.begin(), data.other_objects.end(), + [&object_id](const auto & o) { return o.object.object_id == object_id; }); }; - const auto isIgnoreObject = [&](const auto & r_id) { - const auto & n = other_objects; - return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); - }; - - for (const auto & registered : registered_objects) { - if ( - !isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) { - now_objects.push_back(registered); + for (auto & stored_object : stored_objects) { + if (is_detected(stored_object.object.object_id)) { + continue; } + if (is_ignored(stored_object.object.object_id)) { + continue; + } + + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + fillLongitudinalAndLengthByClosestEnvelopeFootprint( + data.reference_path_rough, ego_pos, stored_object); + + data.target_objects.push_back(stored_object); } } @@ -1807,17 +1876,19 @@ void filterTargetObjects( utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); - // TODO(Satoshi Ota) parametrize stop time threshold if need. - constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] if (filtering_utils::isUnknownTypeObject(o)) { + // TARGET: UNKNOWN + + // TODO(Satoshi Ota) parametrize stop time threshold if need. + constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] if (o.stop_time < STOP_TIME_THRESHOLD) { o.info = ObjectInfo::UNSTABLE_OBJECT; data.other_objects.push_back(o); continue; } - } + } else if (filtering_utils::isVehicleTypeObject(o)) { + // TARGET: CAR, TRUCK, BUS, TRAILER, MOTORCYCLE - if (filtering_utils::isVehicleTypeObject(o)) { o.is_within_intersection = filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = @@ -1834,6 +1905,10 @@ void filterTargetObjects( continue; } } else { + // TARGET: PEDESTRIAN, BICYCLE + + o.is_within_intersection = + filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = false; o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); @@ -1987,6 +2062,17 @@ lanelet::ConstLanelets getAdjacentLane( lanes.push_back(opt_right_lane.value()); } + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lane); + if (!is_right_shift && !left_opposite_lanes.empty()) { + lanes.push_back(left_opposite_lanes.front()); + + for (const auto & prev_lane : rh->getPreviousLanelets(left_opposite_lanes.front())) { + if (!exist(prev_lane.id())) { + lanes.push_back(prev_lane); + } + } + } + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); if (is_right_shift && !right_opposite_lanes.empty()) { lanes.push_back(right_opposite_lanes.front()); @@ -2064,21 +2150,35 @@ std::vector getSafetyCheckTargetObjects( return {}; } + const auto is_moving = [¶meters](const auto & object) { + const auto & object_twist = object.kinematics.initial_twist_with_covariance.twist; + const auto object_vel_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); + const auto object_parameter = + parameters->object_parameters.at(utils::getHighestProbLabel(object.classification)); + return object_vel_norm > object_parameter.moving_speed_threshold; + }; + + const auto filter = + [&is_moving](const auto & object, const auto & lanelet, [[maybe_unused]] const auto unused) { + // filter by yaw deviation only when the object is moving because the head direction is not + // reliable while object is stopping. + const auto yaw_threshold = is_moving(object) ? M_PI_2 : M_PI; + return utils::path_safety_checker::isCentroidWithinLanelet(object, lanelet, yaw_threshold); + }; + // check right lanes if (check_right_lanes) { const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, true); if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); } @@ -2092,15 +2192,13 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); } @@ -2114,15 +2212,13 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(data.other_objects), check_lanes, filter); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, - utils::path_safety_checker::isCentroidWithinLanelet); + to_predicted_objects(unavoidable_objects), check_lanes, filter); append(targets); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 1880adb045b42..d8725f016fda2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -621,14 +621,14 @@ lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); const auto ego_half_lanelet = generateHalfLanelet(lane); - const auto adj = + const auto assoc_adj = turn_direction_ == TurnDirection::LEFT ? (routing_graph_ptr->adjacentLeft(lane)) : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) : boost::none); const std::optional opposite_adj = [&]() -> std::optional { - if (!!adj) { + if (!!assoc_adj) { return std::nullopt; } if (turn_direction_ == TurnDirection::LEFT) { @@ -653,10 +653,27 @@ lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( } }(); - if (!adj && !opposite_adj) { - blind_spot_lanelets.push_back(ego_half_lanelet); - } else if (!!adj) { - const auto adj_half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); + const auto assoc_shoulder = [&]() -> std::optional { + if (turn_direction_ == TurnDirection::LEFT) { + return planner_data_->route_handler_->getLeftShoulderLanelet(lane); + } else if (turn_direction_ == TurnDirection::RIGHT) { + return planner_data_->route_handler_->getRightShoulderLanelet(lane); + } + return std::nullopt; + }(); + if (assoc_shoulder) { + const auto lefts = (turn_direction_ == TurnDirection::LEFT) + ? assoc_shoulder.value().leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction_ == TurnDirection::LEFT) + ? ego_half_lanelet.rightBound() + : assoc_shoulder.value().rightBound(); + blind_spot_lanelets.push_back( + lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); + + } else if (!!assoc_adj) { + const auto adj_half_lanelet = + generateExtendedAdjacentLanelet(assoc_adj.value(), turn_direction_); const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound() : ego_half_lanelet.leftBound(); const auto rights = (turn_direction_ == TurnDirection::RIGHT) ? adj_half_lanelet.rightBound() @@ -672,6 +689,8 @@ lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( : adj_half_lanelet.rightBound(); blind_spot_lanelets.push_back( lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); + } else { + blind_spot_lanelets.push_back(ego_half_lanelet); } } return blind_spot_lanelets; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 786828d72dcb3..1e6658987f522 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -382,7 +382,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( // Check if ego moves forward enough to ignore yield. const auto & p = planner_param_; if (!path_intersects.empty()) { - const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const double dist_ego2crosswalk = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); const auto braking_distance_opt = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index 648fc611ac6b1..943eef03fc6ce 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -48,11 +48,10 @@ void IntersectionLanelets::update( } } if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { - const auto first_attention_lane = first_attention_lane_.value(); // remove first_attention_area_ and non-straight lanelets from attention_non_preceding lanelet::ConstLanelets attention_non_preceding_ex_first; lanelet::ConstLanelets sibling_first_attention_lanelets; - for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane_.value())) { for (const auto & following : routing_graph_ptr->following(previous)) { sibling_first_attention_lanelets.push_back(following); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index 6ead1df3eb2d1..b749207aa2393 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -137,11 +137,9 @@ bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const if (!dist_to_stopline_opt) { return false; } - const double observed_velocity = - predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; const double dist_to_stopline = dist_to_stopline_opt.value(); - const double braking_distance = - (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration); return dist_to_stopline > braking_distance; } @@ -153,10 +151,8 @@ bool ObjectInfo::can_stop_before_ego_lane( return false; } const double dist_to_stopline = dist_to_stopline_opt.value(); - const double observed_velocity = - predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration); if (dist_to_stopline > braking_distance) { return false; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index ac157fb3ab6dc..9f6935e6d7e7b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -392,7 +392,6 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { if (has_collision) { - const auto closest_idx = intersection_stoplines.closest_idx; const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); return OverPassJudge{ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index a4c4e1b41688f..92a8fc57079df 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -689,8 +689,8 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets( routing_graph_ptr, ll, length, ego_lanelets); for (const auto & ls : lanelet_sequences) { for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); + const auto & inner_inserted = detection_ids.insert(l.id()); + if (inner_inserted.second) detection_and_preceding_lanelets.push_back(l); } } } @@ -710,8 +710,8 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets( routing_graph_ptr, ll, length, ego_lanelets); for (const auto & ls : lanelet_sequences) { for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + const auto & inner_inserted = detection_ids.insert(l.id()); + if (inner_inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 42d896fec8ab3..465fd770049e9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -149,7 +149,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR const auto & op_target_point_idx = autoware::motion_utils::insertTargetPoint(target_segment_idx, target_point, path->points, 5e-2); - size_t target_point_idx; + size_t target_point_idx = 0; if (op_target_point_idx) { target_point_idx = op_target_point_idx.value(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 3482d7be8ec48..cc2f4bf3b96b4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -54,7 +54,6 @@ std::shared_ptr generateNode() const auto velocity_smoother_dir = ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - // TODO(esteve): delete when all the modules are migrated to autoware_behavior_velocity_* const auto get_behavior_velocity_module_config = [](const std::string & module) { const auto package_name = "autoware_behavior_velocity_" + module + "_module"; const auto package_path = ament_index_cpp::get_package_share_directory(package_name); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index b09f00ce367dc..29f3794516ef5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -42,6 +43,7 @@ #include // Debug +#include #include #include @@ -106,6 +108,13 @@ class SceneModuleInterface infrastructure_command_ = command; } + void setTimeKeeper(const std::shared_ptr & time_keeper) + { + time_keeper_ = time_keeper; + } + + std::shared_ptr getTimeKeeper() { return time_keeper_; } + std::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } void setActivation(const bool activated) { activated_ = activated; } @@ -132,6 +141,7 @@ class SceneModuleInterface std::optional first_stop_path_point_index_; autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; + mutable std::shared_ptr time_keeper_; void setSafe(const bool safe) { @@ -215,6 +225,10 @@ class SceneModuleManagerInterface pub_infrastructure_commands_; std::shared_ptr processing_time_publisher_; + + rclcpp::Publisher::SharedPtr pub_processing_time_detail_; + + std::shared_ptr time_keeper_; }; class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index a0f30cd3e33cb..350bc439a4a41 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -17,9 +17,11 @@ #include #include #include +#include #include #include +#include namespace autoware::behavior_velocity_planner { @@ -33,7 +35,8 @@ SceneModuleInterface::SceneModuleInterface( safe_(false), distance_(std::numeric_limits::lowest()), logger_(logger), - clock_(clock) + clock_(clock), + time_keeper_(std::shared_ptr()) { } @@ -71,6 +74,11 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( "~/output/infrastructure_commands", 1); processing_time_publisher_ = std::make_shared(&node, "~/debug"); + + pub_processing_time_detail_ = node.create_publisher( + "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); + + time_keeper_ = std::make_shared(pub_processing_time_detail_); } size_t SceneModuleManagerInterface::findEgoSegmentIndex( @@ -94,6 +102,8 @@ void SceneModuleManagerInterface::updateSceneModuleInstances( void SceneModuleManagerInterface::modifyPathVelocity( tier4_planning_msgs::msg::PathWithLaneId * path) { + universe_utils::ScopedTimeTrack st( + "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); StopWatch stop_watch; stop_watch.tic("Total"); visualization_msgs::msg::MarkerArray debug_marker_array; @@ -177,6 +187,7 @@ void SceneModuleManagerInterface::registerModule( RCLCPP_DEBUG( logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_module->setTimeKeeper(time_keeper_); scene_modules_.insert(scene_module); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp index 537d647883ad4..4f03dadc0555a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -166,7 +166,7 @@ void insertPathVelocityFromIndex( } } -std::optional findFirstStopPointIdx(PathPointsWithLaneId & path_points) +std::optional findFirstStopPointIdx(const PathPointsWithLaneId & path_points) { for (size_t i = 0; i < path_points.size(); i++) { const auto vel = path_points.at(i).point.longitudinal_velocity_mps; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index 5f5b7206551ef..f96d9f966265f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -235,7 +235,7 @@ void insertPathVelocityFromIndexLimited( void insertPathVelocityFromIndex( const size_t & start_idx, const float velocity_mps, PathPointsWithLaneId & path_points); -std::optional findFirstStopPointIdx(PathPointsWithLaneId & path_points); +std::optional findFirstStopPointIdx(const PathPointsWithLaneId & path_points); LineString2d createLineString2d(const lanelet::BasicPolygon2d & poly); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 04ea5ca872098..c5079452f5ada 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -40,6 +41,8 @@ StopLineModule::StopLineModule( bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { + universe_utils::ScopedTimeTrack st( + std::string(__func__) + " (lane_id:=" + std::to_string(module_id_) + ")", *getTimeKeeper()); debug_data_ = DebugData(); if (path->points.empty()) return true; const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -50,11 +53,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop const LineString2d stop_line = planning_utils::extendLine( stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); + time_keeper_->start_track("createTargetPoint"); // Calculate stop pose and insert index const auto stop_point = arc_lane_utils::createTargetPoint( *path, stop_line, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); - + time_keeper_->end_track("createTargetPoint"); // If no collision found, do nothing if (!stop_point) { RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision"); @@ -70,12 +74,16 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop * |----------------------------| * s---ego----------x--|--------g */ + time_keeper_->start_track( + "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength"); const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( path->points, stop_pose.position, stop_point_idx); const size_t current_seg_idx = findEgoSegmentIndex(path->points); const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, current_seg_idx, stop_pose.position, stop_line_seg_idx); + time_keeper_->end_track( + "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength"); switch (state_) { case State::APPROACH: { // Insert stop pose diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp index 00e87c69d53fa..63ed073a0983d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -40,7 +40,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, const std::vector & objects, - const autoware::universe_utils::MultiPolygon2d & obstacle_forward_footprints); + const autoware::universe_utils::MultiPolygon2d & object_forward_footprints); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 73189e732b312..8f59160a56f2a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -79,7 +79,7 @@ void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { const auto box = boost::geometry::return_envelope( ego_data.trajectory_footprints[i]); - rtree_nodes.push_back(std::make_pair(box, i)); + rtree_nodes.emplace_back(box, i); } ego_data.rtree = Rtree(rtree_nodes); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/occupancy_grid_utils.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/occupancy_grid_utils.hpp index f1d5b69714772..4acdfa7945fb1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/occupancy_grid_utils.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/occupancy_grid_utils.hpp @@ -28,7 +28,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter /// @brief mask gridmap cells that are inside the given polygons /// @param[in, out] grid_map the grid map to modify /// @param[in] polygons the polygons to mask from the grid map -void maskPolygons(grid_map::GridMap & grid_map, const ObstacleMasks & masks); +void maskPolygons(grid_map::GridMap & grid_map, const ObstacleMasks & obstacle_masks); /// @brief apply a threshold to the grid map /// @param[in, out] grid_map the grid map to modify diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp index f4b1304d67e5e..f56b50635c96e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp @@ -71,7 +71,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b auto prev_point = trajectory.front(); auto prev_heading = tf2::getYaw(prev_point.pose.orientation); for (auto i = 1ul; i < trajectory.size(); ++i) { - const auto & prev_point = trajectory[i - 1]; + prev_point = trajectory[i - 1]; auto & point = trajectory[i]; const auto dt = autoware::universe_utils::calcDistance2d(prev_point, point) / prev_point.longitudinal_velocity_mps; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index bfa38e544227f..d04ae920a7d6a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -36,7 +36,7 @@ lanelet::ConstLanelets consecutive_lanelets( } lanelet::ConstLanelets get_missing_lane_change_lanelets( - lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & trajectory_lanelets, const std::shared_ptr route_handler) { lanelet::ConstLanelets missing_lane_change_lanelets; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp index 8618d808d7d40..94023ae08ad99 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -51,7 +51,7 @@ lanelet::ConstLanelets calculate_trajectory_lanelets( /// @return lanelets that may be overlapped by a lane change (and are not already in /// trajectory_lanelets) lanelet::ConstLanelets get_missing_lane_change_lanelets( - lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & trajectory_lanelets, const std::shared_ptr route_handler); /// @brief calculate lanelets that should be ignored /// @param [in] ego_data data about the ego vehicle diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 07e496d52764b..f181b0c76b51e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -325,7 +325,7 @@ void MotionVelocityPlannerNode::insert_slowdown( autoware::motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points); if (from_insert_idx && to_insert_idx) { for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) - trajectory.points[idx].longitudinal_velocity_mps = 0.0; + trajectory.points[idx].longitudinal_velocity_mps = slowdown_interval.velocity; } else { RCLCPP_WARN(get_logger(), "Failed to insert slowdown point"); } diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index 1f05fc93138a2..1e04db2a17eee 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023-2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -186,7 +186,7 @@ void calculateCartesian( for (size_t i = 1; i < path.yaws.size(); ++i) { const auto dyaw = autoware::common::helper_functions::wrap_angle(path.yaws[i] - path.yaws[i - 1]); - path.curvatures.push_back(dyaw / (path.lengths[i - 1], path.lengths[i])); + path.curvatures.push_back(dyaw / (path.lengths[i] - path.lengths[i - 1])); } path.curvatures.push_back(path.curvatures.back()); } diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 97ca1d8bffe77..033abf04f1255 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -49,29 +49,29 @@ class GNSSPoser : public rclcpp::Node private: using MapProjectorInfo = map_interface::MapProjectorInfo; - void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg); - void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); - void callbackGnssInsOrientationStamped( + void callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + void callback_nav_sat_fix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); + void callback_gnss_ins_orientation_stamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); - bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); - bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); - geometry_msgs::msg::Point getMedianPosition( + static bool is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); + static bool can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); + static geometry_msgs::msg::Point get_median_position( const boost::circular_buffer & position_buffer); - geometry_msgs::msg::Point getAveragePosition( + static geometry_msgs::msg::Point get_average_position( const boost::circular_buffer & position_buffer); - geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); - geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( + static geometry_msgs::msg::Quaternion get_quaternion_by_heading(const int heading); + static geometry_msgs::msg::Quaternion get_quaternion_by_position_difference( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); - bool getTransform( + bool get_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr); - bool getStaticTransform( + bool get_static_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, const builtin_interfaces::msg::Time & stamp); - void publishTF( + void publish_tf( const std::string & frame_id, const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg); @@ -99,7 +99,7 @@ class GNSSPoser : public rclcpp::Node autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; - int gnss_pose_pub_method; + int gnss_pose_pub_method_; }; } // namespace gnss_poser diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 2de081b6b009f..eef6d690b6e72 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -5,7 +5,6 @@ 1.0.0 The ROS 2 gnss_poser package Yamato Ando - Koji Minoda Masahiro Sakamoto Kento Yabuuchi NGUYEN Viet Anh diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 7a3b40336ff3d..40a60d17ea7db 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -36,25 +36,27 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), msg_gnss_ins_orientation_stamped_( std::make_shared()), - gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method")) + gnss_pose_pub_method_(static_cast(declare_parameter("gnss_pose_pub_method"))) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); adaptor.init_sub( - sub_map_projector_info_, - [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); + sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { + callback_map_projector_info(msg); + }); // Set up position buffer - int buff_epoch = declare_parameter("buff_epoch"); + int buff_epoch = static_cast(declare_parameter("buff_epoch")); position_buffer_.set_capacity(buff_epoch); // Set subscribers and publishers nav_sat_fix_sub_ = create_subscription( - "fix", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavSatFix, this, std::placeholders::_1)); + "fix", rclcpp::QoS{1}, + std::bind(&GNSSPoser::callback_nav_sat_fix, this, std::placeholders::_1)); autoware_orientation_sub_ = create_subscription( "autoware_orientation", rclcpp::QoS{1}, - std::bind(&GNSSPoser::callbackGnssInsOrientationStamped, this, std::placeholders::_1)); + std::bind(&GNSSPoser::callback_gnss_ins_orientation_stamped, this, std::placeholders::_1)); pose_pub_ = create_publisher("gnss_pose", rclcpp::QoS{1}); pose_cov_pub_ = create_publisher( @@ -68,13 +70,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z = 1.0; } -void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg) +void GNSSPoser::callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg) { projector_info_ = *msg; received_map_projector_info_ = true; } -void GNSSPoser::callbackNavSatFix( +void GNSSPoser::callback_nav_sat_fix( const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) { // Return immediately if map_projector_info has not been received yet. @@ -94,15 +96,15 @@ void GNSSPoser::callbackNavSatFix( } // check fixed topic - const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); + const bool is_status_fixed = is_fixed(nav_sat_fix_msg_ptr->status); // publish is_fixed topic tier4_debug_msgs::msg::BoolStamped is_fixed_msg; is_fixed_msg.stamp = this->now(); - is_fixed_msg.data = is_fixed; + is_fixed_msg.data = is_status_fixed; fixed_pub_->publish(is_fixed_msg); - if (!is_fixed) { + if (!is_status_fixed) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), "Not Fixed Topic. Skipping Calculate."); @@ -122,7 +124,7 @@ void GNSSPoser::callbackNavSatFix( geometry_msgs::msg::Pose gnss_antenna_pose{}; // publish pose immediately - if (!gnss_pose_pub_method) { + if (!gnss_pose_pub_method_) { gnss_antenna_pose.position = position; } else { // fill position buffer @@ -134,8 +136,9 @@ void GNSSPoser::callbackNavSatFix( return; } // publish average pose or median pose of position buffer - gnss_antenna_pose.position = (gnss_pose_pub_method == 1) ? getAveragePosition(position_buffer_) - : getMedianPosition(position_buffer_); + gnss_antenna_pose.position = (gnss_pose_pub_method_ == 1) + ? get_average_position(position_buffer_) + : get_median_position(position_buffer_); } // calc gnss antenna orientation @@ -144,7 +147,7 @@ void GNSSPoser::callbackNavSatFix( orientation = msg_gnss_ins_orientation_stamped_->orientation.orientation; } else { static auto prev_position = gnss_antenna_pose.position; - orientation = getQuaternionByPositionDifference(gnss_antenna_pose.position, prev_position); + orientation = get_quaternion_by_position_difference(gnss_antenna_pose.position, prev_position); prev_position = gnss_antenna_pose.position; } @@ -157,7 +160,7 @@ void GNSSPoser::callbackNavSatFix( auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id; - getStaticTransform( + get_static_transform( gnss_frame, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); @@ -179,11 +182,11 @@ void GNSSPoser::callbackNavSatFix( gnss_base_pose_cov_msg.header = gnss_base_pose_msg.header; gnss_base_pose_cov_msg.pose.pose = gnss_base_pose_msg.pose; gnss_base_pose_cov_msg.pose.covariance[7 * 0] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; gnss_base_pose_cov_msg.pose.covariance[7 * 1] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; gnss_base_pose_cov_msg.pose.covariance[7 * 2] = - canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; if (use_gnss_ins_orientation_) { gnss_base_pose_cov_msg.pose.covariance[7 * 3] = @@ -201,30 +204,30 @@ void GNSSPoser::callbackNavSatFix( pose_cov_pub_->publish(gnss_base_pose_cov_msg); // broadcast map to gnss_base_link - publishTF(map_frame_, gnss_base_frame_, gnss_base_pose_msg); + publish_tf(map_frame_, gnss_base_frame_, gnss_base_pose_msg); } -void GNSSPoser::callbackGnssInsOrientationStamped( +void GNSSPoser::callback_gnss_ins_orientation_stamped( const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg) { *msg_gnss_ins_orientation_stamped_ = *msg; } -bool GNSSPoser::isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) +bool GNSSPoser::is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) { return nav_sat_status_msg.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; } -bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) +bool GNSSPoser::can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) { return nav_sat_fix_msg.position_covariance_type > sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } -geometry_msgs::msg::Point GNSSPoser::getMedianPosition( +geometry_msgs::msg::Point GNSSPoser::get_median_position( const boost::circular_buffer & position_buffer) { - auto getMedian = [](std::vector array) { + auto get_median = [](std::vector array) { std::sort(std::begin(array), std::end(array)); const size_t median_index = array.size() / 2; double median = (array.size() % 2) @@ -243,13 +246,13 @@ geometry_msgs::msg::Point GNSSPoser::getMedianPosition( } geometry_msgs::msg::Point median_point; - median_point.x = getMedian(array_x); - median_point.y = getMedian(array_y); - median_point.z = getMedian(array_z); + median_point.x = get_median(array_x); + median_point.y = get_median(array_y); + median_point.z = get_median(array_z); return median_point; } -geometry_msgs::msg::Point GNSSPoser::getAveragePosition( +geometry_msgs::msg::Point GNSSPoser::get_average_position( const boost::circular_buffer & position_buffer) { std::vector array_x; @@ -271,7 +274,7 @@ geometry_msgs::msg::Point GNSSPoser::getAveragePosition( return average_point; } -geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByHeading(const int heading) +geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_heading(const int heading) { int heading_conv = 0; // convert heading[0(North)~360] to yaw[-M_PI(West)~M_PI] @@ -288,7 +291,7 @@ geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByHeading(const int headi return tf2::toMsg(quaternion); } -geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByPositionDifference( +geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_position_difference( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point) { const double yaw = std::atan2(point.y - prev_point.y, point.x - prev_point.x); @@ -297,7 +300,7 @@ geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByPositionDifference( return tf2::toMsg(quaternion); } -bool GNSSPoser::getTransform( +bool GNSSPoser::get_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr) { @@ -340,7 +343,7 @@ bool GNSSPoser::getTransform( return true; } -bool GNSSPoser::getStaticTransform( +bool GNSSPoser::get_static_transform( const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, const builtin_interfaces::msg::Time & stamp) @@ -385,7 +388,7 @@ bool GNSSPoser::getStaticTransform( return true; } -void GNSSPoser::publishTF( +void GNSSPoser::publish_tf( const std::string & frame_id, const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg) { diff --git a/sensing/image_diagnostics/CMakeLists.txt b/sensing/image_diagnostics/CMakeLists.txt index 800cd3f67d902..186d9340dc08a 100644 --- a/sensing/image_diagnostics/CMakeLists.txt +++ b/sensing/image_diagnostics/CMakeLists.txt @@ -28,14 +28,14 @@ include_directories( ) -ament_auto_add_library(image_diagnostics SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/image_diagnostics_node.cpp ) -target_link_libraries(image_diagnostics +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ) -rclcpp_components_register_node(image_diagnostics - PLUGIN "image_diagnostics::ImageDiagNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::image_diagnostics::ImageDiagNode" EXECUTABLE image_diagnostics_node ) diff --git a/sensing/image_diagnostics/src/image_diagnostics_node.cpp b/sensing/image_diagnostics/src/image_diagnostics_node.cpp index 3db9ab94f50fa..49204f5cbc756 100644 --- a/sensing/image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/image_diagnostics/src/image_diagnostics_node.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_diagnostics/image_diagnostics_node.hpp" +#include "image_diagnostics_node.hpp" #include -namespace image_diagnostics +#include + +namespace autoware::image_diagnostics { using image_diagnostics::Image_State; ImageDiagNode::ImageDiagNode(const rclcpp::NodeOptions & node_options) @@ -271,7 +273,7 @@ void ImageDiagNode::shiftImage(cv::Mat & img) tmp.copyTo(left_bottom); } -} // namespace image_diagnostics +} // namespace autoware::image_diagnostics #include -RCLCPP_COMPONENTS_REGISTER_NODE(image_diagnostics::ImageDiagNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::image_diagnostics::ImageDiagNode) diff --git a/sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp b/sensing/image_diagnostics/src/image_diagnostics_node.hpp similarity index 93% rename from sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp rename to sensing/image_diagnostics/src/image_diagnostics_node.hpp index d6d150220555f..325624062b90b 100644 --- a/sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp +++ b/sensing/image_diagnostics/src/image_diagnostics_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_DIAGNOSTICS__IMAGE_DIAGNOSTICS_NODE_HPP_ -#define IMAGE_DIAGNOSTICS__IMAGE_DIAGNOSTICS_NODE_HPP_ +#ifndef IMAGE_DIAGNOSTICS_NODE_HPP_ +#define IMAGE_DIAGNOSTICS_NODE_HPP_ #include #include @@ -34,7 +34,8 @@ #include #include -namespace image_diagnostics + +namespace autoware::image_diagnostics { using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; @@ -98,6 +99,6 @@ class ImageDiagNode : public rclcpp::Node rclcpp::Publisher::SharedPtr image_state_pub_; }; -} // namespace image_diagnostics +} // namespace autoware::image_diagnostics -#endif // IMAGE_DIAGNOSTICS__IMAGE_DIAGNOSTICS_NODE_HPP_ +#endif // IMAGE_DIAGNOSTICS_NODE_HPP_ diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index cbc28e2ff0eeb..f5c719d2b4f7e 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -6,7 +6,6 @@ The ROS 2 imu_corrector package Yamato Ando Taiki Yamada - Koji Minoda Apache License 2.0 Yamato Ando diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 278d2dfc28e59..6882fbf068fd1 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -51,11 +51,11 @@ GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) [this](const Odometry::ConstSharedPtr msg) { callback_odom(msg); }); gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); - auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); + auto bound_timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); auto period_control = std::chrono::duration_cast( std::chrono::duration(timer_callback_interval_sec_)); - timer_ = std::make_shared>( - this->get_clock(), period_control, std::move(timer_callback), + timer_ = std::make_shared>( + this->get_clock(), period_control, std::move(bound_timer_callback), this->get_node_base_interface()->get_context()); this->get_node_timers_interface()->add_timer(timer_, nullptr); diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 2d0649fe43954..76b9cc91ea586 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -26,11 +26,13 @@ include_directories( add_library(pointcloud_preprocessor_filter_base SHARED src/filter.cpp + src/utility/memory.cpp ) target_include_directories(pointcloud_preprocessor_filter_base PUBLIC "$" "$" + ${autoware_point_types_INCLUDE_DIRS} ) ament_target_dependencies(pointcloud_preprocessor_filter_base @@ -41,6 +43,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base tf2_ros autoware_universe_utils pcl_ros + autoware_point_types ) add_library(faster_voxel_grid_downsample_filter SHARED @@ -59,7 +62,6 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter ) ament_auto_add_library(pointcloud_preprocessor_filter SHARED - src/utility/utilities.cpp src/concatenate_data/concatenate_and_time_sync_nodelet.cpp src/concatenate_data/concatenate_pointclouds.cpp src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -82,6 +84,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/blockage_diag/blockage_diag_nodelet.cpp src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter.cpp + src/utility/geometry.cpp ) target_link_libraries(pointcloud_preprocessor_filter @@ -227,16 +230,19 @@ ament_auto_package(INSTALL_TO_SHARE if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_utilities test/test_utilities.cpp ) - ament_add_gtest(distortion_corrector_node_tests - test/test_distortion_corrector_node.cpp + + ament_add_gtest(test_distortion_corrector_node + test/test_distortion_corrector_node.cpp ) target_link_libraries(test_utilities pointcloud_preprocessor_filter) - target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter) + target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter) endif() diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 96124f473f825..033e82607057a 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -36,7 +36,7 @@ Please note that the processing time difference between the two distortion metho ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector.schema.json") }} +{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json") }} ## Launch diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md index c5a0b75e9b33b..d5993a803fa87 100644 --- a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -10,7 +10,7 @@ This node can remove rain and fog by considering the light reflected from the ob ![outlier_filter-return_type](./image/outlier_filter-return_type.drawio.svg) -Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRADRT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L57-L76) data structure. +Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116) data structure. Another feature of this node is that it publishes visibility as a diagnostic topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle. @@ -73,7 +73,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ## Assumptions / Known limits Not recommended for use as it is under development. -Input data must be `PointXYZIRADRT` type data including `return_type`. +Input data must be [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116) type data including `return_type`. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index e87023ef00149..0a179ddf6a8ef 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -73,16 +73,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ## Assumptions / Known limits -It is a prerequisite to input a scan point cloud in chronological order. In this repository it is defined as blow structure (please refer to [PointXYZIRADT](https://github.com/tier4/AutowareArchitectureProposal.iv/blob/5d8dff0db51634f0c42d2a3e87ca423fbee84348/sensing/preprocessor/pointcloud/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp#L53-L62)). - -- X: x -- Y: y -- z: z -- I: intensity -- R: ring -- A :azimuth -- D: distance -- T: time_stamp +This nodes requires that the points of the input point cloud are in chronological order and that individual points follow the memory layout specified by [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116). ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 86c4ed5943540..619a0b8520b97 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -87,7 +87,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data @@ -169,7 +169,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index ead66d98b8be7..77717c51e6981 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -86,7 +86,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; /** \brief @b PointCloudConcatenationComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, @@ -160,7 +160,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 6f372f0e74646..e37329a9a4cc3 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -48,6 +48,11 @@ namespace pointcloud_preprocessor class DistortionCorrectorBase { public: + virtual bool pointcloud_transform_exists() = 0; + virtual bool pointcloud_transform_needed() = 0; + virtual std::deque get_twist_queue() = 0; + virtual std::deque get_angular_velocity_queue() = 0; + virtual void processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; virtual void processIMUMessage( @@ -61,7 +66,8 @@ class DistortionCorrectorBase template class DistortionCorrector : public DistortionCorrectorBase { -public: +protected: + geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr_; bool pointcloud_transform_needed_{false}; bool pointcloud_transform_exists_{false}; bool imu_transform_exists_{false}; @@ -72,28 +78,12 @@ class DistortionCorrector : public DistortionCorrectorBase std::deque twist_queue_; std::deque angular_velocity_queue_; - explicit DistortionCorrector(rclcpp::Node * node) - : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) - { - } - void processTwistMessage( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; - - void processIMUMessage( - const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; - void getIMUTransformation( - const std::string & base_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - void enqueueIMU( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr); - - bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); + void getIMUTransformation(const std::string & base_frame, const std::string & imu_frame); + void enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); void getTwistAndIMUIterator( bool use_imu, double first_point_time_stamp_sec, std::deque::iterator & it_twist, std::deque::iterator & it_imu); - void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; void warnIfTimestampIsTooLate(bool is_twist_time_stamp_too_late, bool is_imu_time_stamp_too_late); void undistortPoint( sensor_msgs::PointCloud2Iterator & it_x, sensor_msgs::PointCloud2Iterator & it_y, @@ -105,6 +95,23 @@ class DistortionCorrector : public DistortionCorrectorBase static_cast(this)->undistortPointImplementation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; + +public: + explicit DistortionCorrector(rclcpp::Node * node) + : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) + { + } + bool pointcloud_transform_exists(); + bool pointcloud_transform_needed(); + std::deque get_twist_queue(); + std::deque get_angular_velocity_queue(); + void processTwistMessage( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; + + void processIMUMessage( + const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) override; + void undistortPointCloud(bool use_imu, sensor_msgs::msg::PointCloud2 & pointcloud) override; + bool isInputValid(sensor_msgs::msg::PointCloud2 & pointcloud); }; class DistortionCorrector2D : public DistortionCorrector diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp index d80e62d08330d..379c4c79e555a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp @@ -34,7 +34,7 @@ class FasterVoxelGridDownsampleFilter public: FasterVoxelGridDownsampleFilter(); void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z); - void set_field_offsets(const PointCloud2ConstPtr & input); + void set_field_offsets(const PointCloud2ConstPtr & input, const rclcpp::Logger & logger); void filter( const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, const rclcpp::Logger & logger); @@ -48,7 +48,7 @@ class FasterVoxelGridDownsampleFilter float intensity; uint32_t point_count_; - Centroid() : x(0), y(0), z(0), intensity(-1.0) {} + Centroid() : x(0), y(0), z(0), intensity(0) {} Centroid(float _x, float _y, float _z, float _intensity) : x(_x), y(_y), z(_z), intensity(_intensity) { diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index 55ba79aac4593..08fbfe73f2744 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -35,12 +35,15 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter { protected: + using InputPointIndex = autoware_point_types::PointXYZIRCAEDTIndex; + using InputPointType = autoware_point_types::PointXYZIRCAEDT; + using OutputPointType = autoware_point_types::PointXYZIRC; + virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); @@ -83,8 +86,10 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter { if (walk_size > num_points_threshold_) return true; - auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); - auto last_point = reinterpret_cast(&input->data[data_idx_both_ends.second]); + auto first_point = + reinterpret_cast(&input->data[data_idx_both_ends.first]); + auto last_point = + reinterpret_cast(&input->data[data_idx_both_ends.second]); const auto x = first_point->x - last_point->x; const auto y = first_point->y - last_point->y; @@ -94,8 +99,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter } void setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, - size_t num_fields); + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size); float calculateVisibilityScore(const PointCloud2 & input); public: diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp index 49baed4ed7ed5..4d2e66eea700e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp @@ -16,7 +16,7 @@ #define POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 4153cee7695f8..2dc98a571ff2c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -87,7 +87,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; // cspell:ignore Yoshi /** \brief @b PointCloudDataSynchronizerComponent is a special form of data @@ -166,7 +166,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node std::map synchronizeClouds(); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp similarity index 78% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp rename to sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp index 654057e7bb8d7..0175f88ceb89c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ -#define POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ +#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ +#define POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ #include #include @@ -81,14 +81,6 @@ void remove_polygon_cgal_from_cloud( bool point_within_cgal_polys( const pcl::PointXYZ & point, const std::vector & polyline_polygons); -/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to - * say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ -bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud2 & input); - -/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That - * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ -bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input); - } // namespace pointcloud_preprocessor::utils -#endif // POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ +#endif // POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp new file mode 100644 index 0000000000000..ef87a4f31457b --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ +#define POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ + +#include + +namespace pointcloud_preprocessor::utils +{ +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to + * say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ +bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input); + +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRC. That is + * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRC */ +bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input); + +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That + * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ +bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input); + +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT. That + * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */ +bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input); + +} // namespace pointcloud_preprocessor::utils + +#endif // POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 7f48497d7e578..d28a6550123cf 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,7 +16,7 @@ #define POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include #include diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 773e6db056990..aae50a18f2024 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -21,7 +21,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZIRADRT; +using autoware_point_types::PointXYZIRCAEDT; using diagnostic_msgs::msg::DiagnosticStatus; BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options) @@ -175,7 +175,7 @@ void BlockageDiagComponent::filter( } ideal_horizontal_bins = static_cast( (angle_range_deg_[1] + compensate_angle - angle_range_deg_[0]) / horizontal_resolution_); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); cv::Mat full_size_depth_map( cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); @@ -196,7 +196,7 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { for (const auto p : pcl_input->points) { - double azimuth_deg = p.azimuth / 100.; + double azimuth_deg = p.azimuth * (180.0 / M_PI); if ( ((azimuth_deg > angle_range_deg_[0]) && (azimuth_deg <= angle_range_deg_[1] + compensate_angle)) || @@ -208,9 +208,9 @@ void BlockageDiagComponent::filter( uint16_t depth_intensity = UINT16_MAX * (1.0 - std::min(p.distance / max_distance_range_, 1.0)); if (is_channel_order_top2down_) { - full_size_depth_map.at(p.ring, horizontal_bin_index) = depth_intensity; + full_size_depth_map.at(p.channel, horizontal_bin_index) = depth_intensity; } else { - full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin_index) = + full_size_depth_map.at(vertical_bins - p.channel - 1, horizontal_bin_index) = depth_intensity; } } diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 8799a70c01736..c550c9dfb4933 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -51,6 +51,8 @@ #include "pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -491,40 +493,56 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( +void PointCloudConcatenateDataSynchronizerComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -549,15 +567,28 @@ void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { + if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), + "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); + } + + return; + } + std::lock_guard lock(mutex_); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); auto input = std::make_shared(*input_ptr); if (input->data.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); } else { - // convert to XYZI pointcloud if pointcloud is not empty - convertToXYZICloud(input, xyzi_input_ptr); + // convert to XYZIRC pointcloud if pointcloud is not empty + convertToXYZIRCCloud(input, xyzirc_input_ptr); } const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); @@ -566,7 +597,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -579,7 +610,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index 89e058d11bb53..d1f02f5f0bbf1 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -14,6 +14,8 @@ #include "pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -325,39 +327,56 @@ void PointCloudConcatenationComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenationComponent::convertToXYZICloud( +void PointCloudConcatenationComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -382,10 +401,23 @@ void PointCloudConcatenationComponent::setPeriod(const int64_t new_period) void PointCloudConcatenationComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { + if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), + "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); + } + + return; + } + std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); - convertToXYZICloud(input, xyzi_input_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZIRCCloud(input, xyzirc_input_ptr); const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); const bool is_already_subscribed_tmp = std::any_of( @@ -393,7 +425,7 @@ void PointCloudConcatenationComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -406,7 +438,7 @@ void PointCloudConcatenationComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 81c29a9f6202a..4a7152a3cd38a 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -14,12 +14,38 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include "autoware/universe_utils/math/trigonometry.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" +#include #include namespace pointcloud_preprocessor { + +template +bool DistortionCorrector::pointcloud_transform_exists() +{ + return pointcloud_transform_exists_; +} + +template +bool DistortionCorrector::pointcloud_transform_needed() +{ + return pointcloud_transform_needed_; +} + +template +std::deque DistortionCorrector::get_twist_queue() +{ + return twist_queue_; +} + +template +std::deque DistortionCorrector::get_angular_velocity_queue() +{ + return angular_velocity_queue_; +} + template void DistortionCorrector::processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) @@ -47,21 +73,20 @@ template void DistortionCorrector::processIMUMessage( const std::string & base_frame, const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr = - std::make_shared(); - getIMUTransformation(base_frame, imu_msg->header.frame_id, geometry_imu_to_base_link_ptr); - enqueueIMU(imu_msg, geometry_imu_to_base_link_ptr); + getIMUTransformation(base_frame, imu_msg->header.frame_id); + enqueueIMU(imu_msg); } template void DistortionCorrector::getIMUTransformation( - const std::string & base_frame, const std::string & imu_frame, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) + const std::string & base_frame, const std::string & imu_frame) { if (imu_transform_exists_) { return; } + geometry_imu_to_base_link_ptr_ = std::make_shared(); + tf2::Transform tf2_imu_to_base_link; if (base_frame == imu_frame) { tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); @@ -83,20 +108,18 @@ void DistortionCorrector::getIMUTransformation( } } - geometry_imu_to_base_link_ptr->transform.rotation = + geometry_imu_to_base_link_ptr_->transform.rotation = tf2::toMsg(tf2_imu_to_base_link.getRotation()); } template -void DistortionCorrector::enqueueIMU( - const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg, - geometry_msgs::msg::TransformStamped::SharedPtr geometry_imu_to_base_link_ptr) +void DistortionCorrector::enqueueIMU(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { geometry_msgs::msg::Vector3Stamped angular_velocity; angular_velocity.vector = imu_msg->angular_velocity; geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr); + tf2::doTransform(angular_velocity, transformed_angular_velocity, *geometry_imu_to_base_link_ptr_); transformed_angular_velocity.header = imu_msg->header; angular_velocity_queue_.push_back(transformed_angular_velocity); @@ -159,6 +182,22 @@ bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointc "Required field time stamp doesn't exist in the point cloud."); return false; } + + if (!utils::is_data_layout_compatible_with_point_xyzircaedt(pointcloud)) { + RCLCPP_ERROR( + node_->get_logger(), + "The pointcloud layout is not compatible with PointXYZIRCAEDT. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyziradrt(pointcloud)) { + RCLCPP_ERROR( + node_->get_logger(), + "The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy " + "code/data"); + } + + return false; + } + return true; } @@ -171,10 +210,12 @@ void DistortionCorrector::undistortPointCloud( sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); - sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); + sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); - double prev_time_stamp_sec{*it_time_stamp}; - const double first_point_time_stamp_sec{*it_time_stamp}; + double prev_time_stamp_sec{ + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)}; + const double first_point_time_stamp_sec{ + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)}; std::deque::iterator it_twist; std::deque::iterator it_imu; @@ -192,29 +233,33 @@ void DistortionCorrector::undistortPointCloud( bool is_imu_time_stamp_too_late = false; bool is_twist_valid = true; bool is_imu_valid = true; + double global_point_stamp; for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { is_twist_valid = true; is_imu_valid = true; + global_point_stamp = + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp); + // Get closest twist information - while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { + while (it_twist != std::end(twist_queue_) - 1 && global_point_stamp > twist_stamp) { ++it_twist; twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); } - if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { + if (std::abs(global_point_stamp - twist_stamp) > 0.1) { is_twist_time_stamp_too_late = true; is_twist_valid = false; } // Get closest IMU information if (use_imu && !angular_velocity_queue_.empty()) { - while (it_imu != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { + while (it_imu != std::end(angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) { ++it_imu; imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } - if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { + if (std::abs(global_point_stamp - imu_stamp) > 0.1) { is_imu_time_stamp_too_late = true; is_imu_valid = false; } @@ -222,12 +267,12 @@ void DistortionCorrector::undistortPointCloud( is_imu_valid = false; } - float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); + float time_offset = static_cast(global_point_stamp - prev_time_stamp_sec); // Undistort a single point based on the strategy undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); - prev_time_stamp_sec = *it_time_stamp; + prev_time_stamp_sec = global_point_stamp; } warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); @@ -352,7 +397,9 @@ inline void DistortionCorrector2D::undistortPointImplementation( theta_ += w * time_offset; baselink_quat_.setValue( 0, 0, autoware::universe_utils::sin(theta_ * 0.5f), - autoware::universe_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + autoware::universe_utils::cos( + theta_ * + 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the value is slightly different) const float dis = v * time_offset; x_ += dis * autoware::universe_utils::cos(theta_); y_ += dis * autoware::universe_utils::sin(theta_); diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 0ca28f5f9a455..b9488d1ed7a73 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -29,16 +29,27 @@ void FasterVoxelGridDownsampleFilter::set_voxel_size( Eigen::Array3f::Ones() / Eigen::Array3f(voxel_size_x, voxel_size_y, voxel_size_z); } -void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPtr & input) +void FasterVoxelGridDownsampleFilter::set_field_offsets( + const PointCloud2ConstPtr & input, const rclcpp::Logger & logger) { x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; intensity_index_ = pcl::getFieldIndex(*input, "intensity"); + + if ( + intensity_index_ < 0 || input->fields[pcl::getFieldIndex(*input, "intensity")].datatype != + sensor_msgs::msg::PointField::UINT8) { + RCLCPP_ERROR( + logger, + "There is no intensity field in the input point cloud or the intensity field is not of type " + "UINT8."); + } + if (intensity_index_ != -1) { intensity_offset_ = input->fields[intensity_index_].offset; } else { - intensity_offset_ = z_offset_ + sizeof(float); + intensity_offset_ = -1; } offset_initialized_ = true; } @@ -49,7 +60,7 @@ void FasterVoxelGridDownsampleFilter::filter( { // Check if the field offset has been set if (!offset_initialized_) { - set_field_offsets(input); + set_field_offsets(input, logger); } // Compute the minimum and maximum voxel coordinates @@ -85,10 +96,9 @@ Eigen::Vector4f FasterVoxelGridDownsampleFilter::get_point_from_global_offset( const PointCloud2ConstPtr & input, size_t global_offset) { float intensity = 0.0; - if (intensity_index_ == -1) { - intensity = -1.0; - } else { - intensity = *reinterpret_cast(&input->data[global_offset + intensity_offset_]); + if (intensity_index_ >= 0) { + intensity = static_cast( + *reinterpret_cast(&input->data[global_offset + intensity_offset_])); } Eigen::Vector4f point( *reinterpret_cast(&input->data[global_offset + x_offset_]), @@ -179,7 +189,8 @@ void FasterVoxelGridDownsampleFilter::copy_centroids_to_output( *reinterpret_cast(&output.data[output_data_size + x_offset_]) = centroid[0]; *reinterpret_cast(&output.data[output_data_size + y_offset_]) = centroid[1]; *reinterpret_cast(&output.data[output_data_size + z_offset_]) = centroid[2]; - *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = centroid[3]; + *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = + static_cast(centroid[3]); output_data_size += output.point_step; } } diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h index 02fa183e5d017..edbc55c8158c0 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -126,10 +126,8 @@ static Counts & counts() // bitness #if SIZE_MAX == UINT32_MAX #define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 32 -#elif SIZE_MAX == UINT64_MAX -#define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 #else -#error Unsupported bitness +#define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 #endif // endianess @@ -165,12 +163,13 @@ static Counts & counts() #endif #include #pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) -#define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ - [](size_t mask) noexcept -> int { \ - unsigned long index; \ // NOLINT - return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ - : ROBIN_HOOD(BITNESS); \ - }(x) +#define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ + [](size_t mask) noexcept -> int { \ + unsigned long index; \ // NOLINT + return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) \ // NOLINT + ? static_cast(index) \ // cppcheck-suppress syntaxError + : ROBIN_HOOD(BITNESS); \ // cppcheck-suppress syntaxError + }(x) // cppcheck-suppress syntaxError #else #if ROBIN_HOOD(BITNESS) == 32 #define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzl @@ -429,14 +428,6 @@ class BulkPoolAllocator return *this; } - BulkPoolAllocator & - // NOLINTNEXTLINE (bugprone-unhandled-self-assignment,cert-oop54-cpp) - operator=(const BulkPoolAllocator & ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept - { - // does not do anything - return *this; - } - ~BulkPoolAllocator() noexcept { reset(); } // Deallocates all allocated memory. @@ -475,22 +466,6 @@ class BulkPoolAllocator mHead = obj; } - // Adds an already allocated block of memory to the allocator. This allocator is from now on - // responsible for freeing the data (with free()). If the provided data is not large enough to - // make use of, it is immediately freed. Otherwise it is reused and freed in the destructor. - void addOrFree(void * ptr, const size_t numBytes) noexcept - { - // calculate number of available elements in ptr - if (numBytes < ALIGNMENT + ALIGNED_SIZE) { - // not enough data for at least one element. Free and return. - ROBIN_HOOD_LOG("std::free") - std::free(ptr); - } else { - ROBIN_HOOD_LOG("add to buffer") - add(ptr, numBytes); - } - } - void swap(BulkPoolAllocator & other) noexcept { using std::swap; @@ -1382,7 +1357,7 @@ class Table Iter & operator++() noexcept { mInfo++; - mKeyVals++; + ++mKeyVals; fastForward(); return *this; } @@ -2035,41 +2010,6 @@ class Table return emplace(std::move(keyval)).first; } - // Returns 1 if key is found, 0 otherwise. - size_t count(const key_type & key) const - { // NOLINT (modernize-use-nodiscard) - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { - return 1; - } - return 0; - } - - template - // NOLINTNEXTLINE (modernize-use-nodiscard) - typename std::enable_if::type count(const OtherKey & key) const - { - ROBIN_HOOD_TRACE(this) - auto kv = mKeyVals + findIdx(key); - if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { - return 1; - } - return 0; - } - - bool contains(const key_type & key) const - { // NOLINT (modernize-use-nodiscard) - return 1U == count(key); - } - - template - // NOLINTNEXTLINE (modernize-use-nodiscard) - typename std::enable_if::type contains(const OtherKey & key) const - { - return 1U == count(key); - } - // Returns a reference to the value found for key. // Throws std::out_of_range if element cannot be found template diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index f11f37397a142..6e7469ff05f67 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -108,7 +108,7 @@ void VoxelGridDownsampleFilterComponent::faster_filter( std::scoped_lock lock(mutex_); FasterVoxelGridDownsampleFilter faster_voxel_filter; faster_voxel_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_); - faster_voxel_filter.set_field_offsets(input); + faster_voxel_filter.set_field_offsets(input, this->get_logger()); faster_voxel_filter.filter(input, output, transform_info, this->get_logger()); } diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index 035be38f4c342..435cecec8b6f2 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -51,6 +51,8 @@ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -411,6 +413,37 @@ bool pointcloud_preprocessor::Filter::convert_output_costly(std::unique_ptrget_logger(), "[input_indices_callback] Invalid input!"); return; diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index ea16b7700e5e3..37374d9ad246c 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -28,7 +28,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZIRADRT; +using autoware_point_types::PointXYZIRCAEDT; using autoware_point_types::ReturnType; using diagnostic_msgs::msg::DiagnosticStatus; @@ -110,22 +110,22 @@ void DualReturnOutlierFilterComponent::filter( if (indices) { RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); } - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); uint32_t vertical_bins = vertical_bins_; uint32_t horizontal_bins = 36; - float max_azimuth = 36000.0f; + float max_azimuth = 2 * M_PI; float min_azimuth = 0.0f; switch (roi_mode_map_[roi_mode_]) { case 2: { - max_azimuth = max_azimuth_deg_ * 100.0; - min_azimuth = min_azimuth_deg_ * 100.0; + max_azimuth = max_azimuth_deg_ * (M_PI / 180.0); + min_azimuth = min_azimuth_deg_ * (M_PI / 180.0); break; } default: { - max_azimuth = 36000.0f; + max_azimuth = 2 * M_PI; min_azimuth = 0.0f; break; } @@ -134,13 +134,13 @@ void DualReturnOutlierFilterComponent::filter( uint32_t horizontal_resolution = static_cast((max_azimuth - min_azimuth) / horizontal_bins); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl_output->points.reserve(pcl_input->points.size()); - std::vector> pcl_input_ring_array; - std::vector> weak_first_pcl_input_ring_array; + std::vector> pcl_input_ring_array; + std::vector> weak_first_pcl_input_ring_array; - pcl::PointCloud::Ptr noise_output(new pcl::PointCloud); + pcl::PointCloud::Ptr noise_output(new pcl::PointCloud); noise_output->points.reserve(pcl_input->points.size()); pcl_input_ring_array.resize( vertical_bins); // TODO(davidw): this is for Pandar 40 only, make dynamic @@ -149,9 +149,9 @@ void DualReturnOutlierFilterComponent::filter( // Split into 36 x 10 degree bins x 40 lines (TODO: change to dynamic) for (const auto & p : pcl_input->points) { if (p.return_type == ReturnType::DUAL_WEAK_FIRST) { - weak_first_pcl_input_ring_array.at(p.ring).push_back(p); + weak_first_pcl_input_ring_array.at(p.channel).push_back(p); } else { - pcl_input_ring_array.at(p.ring).push_back(p); + pcl_input_ring_array.at(p.channel).push_back(p); } } @@ -164,16 +164,16 @@ void DualReturnOutlierFilterComponent::filter( } std::vector deleted_azimuths; std::vector deleted_distances; - pcl::PointCloud temp_segment; + pcl::PointCloud temp_segment; bool keep_next = false; - uint ring_id = weak_first_single_ring.points.front().ring; + uint ring_id = weak_first_single_ring.points.front().channel; for (auto iter = std::begin(weak_first_single_ring) + 1; iter != std::end(weak_first_single_ring) - 1; ++iter) { const float min_dist = std::min(iter->distance, (iter + 1)->distance); const float max_dist = std::max(iter->distance, (iter + 1)->distance); float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; if (max_dist < min_dist * weak_first_distance_ratio_ && azimuth_diff < max_azimuth_diff) { temp_segment.points.push_back(*iter); @@ -283,14 +283,14 @@ void DualReturnOutlierFilterComponent::filter( if (input_ring.size() < 2) { continue; } - pcl::PointCloud temp_segment; + pcl::PointCloud temp_segment; bool keep_next = false; // uint ring_id = input_ring.points.front().ring; for (auto iter = std::begin(input_ring) + 1; iter != std::end(input_ring) - 1; ++iter) { const float min_dist = std::min(iter->distance, (iter + 1)->distance); const float max_dist = std::max(iter->distance, (iter + 1)->distance); float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; if (max_dist < min_dist * general_distance_ratio_ && azimuth_diff < max_azimuth_diff) { temp_segment.points.push_back(*iter); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index a7c9e343a00e1..5e783f1e07418 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -82,24 +82,22 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - output.point_step = sizeof(PointXYZI); + output.point_step = sizeof(OutputPointType); output.data.resize(output.point_step * input->width); size_t output_size = 0; - pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); - - const auto ring_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; - const auto azimuth_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; - const auto distance_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; - const auto intensity_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; - const auto return_type_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)).offset; - const auto time_stamp_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).offset; + pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); + + const auto input_channel_offset = + input->fields.at(static_cast(InputPointIndex::Channel)).offset; + const auto input_azimuth_offset = + input->fields.at(static_cast(InputPointIndex::Azimuth)).offset; + const auto input_distance_offset = + input->fields.at(static_cast(InputPointIndex::Distance)).offset; + const auto input_intensity_offset = + input->fields.at(static_cast(InputPointIndex::Intensity)).offset; + const auto input_return_type_offset = + input->fields.at(static_cast(InputPointIndex::ReturnType)).offset; std::vector> ring2indices; ring2indices.reserve(max_rings_num_); @@ -110,7 +108,8 @@ void RingOutlierFilterComponent::faster_filter( } for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { - const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); + const uint16_t ring = + *reinterpret_cast(&input->data[data_idx + input_channel_offset]); ring2indices[ring].push_back(data_idx); } @@ -132,30 +131,30 @@ void RingOutlierFilterComponent::faster_filter( // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) const float & current_azimuth = - *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); + *reinterpret_cast(&input->data[current_data_idx + input_azimuth_offset]); const float & next_azimuth = - *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); + *reinterpret_cast(&input->data[next_data_idx + input_azimuth_offset]); float azimuth_diff = next_azimuth - current_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; const float & current_distance = - *reinterpret_cast(&input->data[current_data_idx + distance_offset]); + *reinterpret_cast(&input->data[current_data_idx + input_distance_offset]); const float & next_distance = - *reinterpret_cast(&input->data[next_data_idx + distance_offset]); + *reinterpret_cast(&input->data[next_data_idx + input_distance_offset]); if ( std::max(current_distance, next_distance) < std::min(current_distance, next_distance) * distance_ratio_ && - azimuth_diff < 100.f) { - continue; // Determined to be included in the same walk + azimuth_diff < 1.0 * (180.0 / M_PI)) { // one degree + continue; // Determined to be included in the same walk } if (isCluster( input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), walk_last_idx - walk_first_idx + 1)) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); @@ -164,41 +163,38 @@ void RingOutlierFilterComponent::faster_filter( output_ptr->y = p[1]; output_ptr->z = p[2]; } else { - *output_ptr = *input_ptr; + output_ptr->x = input_ptr->x; + output_ptr->y = input_ptr->y; + output_ptr->z = input_ptr->z; } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + const std::uint8_t & intensity = *reinterpret_cast( + &input->data[indices[i] + input_intensity_offset]); output_ptr->intensity = intensity; + const std::uint8_t & return_type = *reinterpret_cast( + &input->data[indices[i] + input_return_type_offset]); + output_ptr->return_type = return_type; + + const std::uint8_t & channel = *reinterpret_cast( + &input->data[indices[i] + input_channel_offset]); + output_ptr->channel = channel; + output_size += output.point_step; } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - PointXYZIRADRT outlier_point; auto input_ptr = - reinterpret_cast(&input->data[indices[walk_first_idx]]); + reinterpret_cast(&input->data[indices[walk_first_idx]]); + InputPointType outlier_point = *input_ptr; + if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; outlier_point.x = p[0]; outlier_point.y = p[1]; outlier_point.z = p[2]; - } else { - outlier_point = *input_ptr; } - outlier_point.intensity = *reinterpret_cast( - &input->data[indices[walk_first_idx] + intensity_offset]); - outlier_point.ring = *reinterpret_cast( - &input->data[indices[walk_first_idx] + ring_offset]); - outlier_point.azimuth = *reinterpret_cast( - &input->data[indices[walk_first_idx] + azimuth_offset]); - outlier_point.distance = *reinterpret_cast( - &input->data[indices[walk_first_idx] + distance_offset]); - outlier_point.return_type = *reinterpret_cast( - &input->data[indices[walk_first_idx] + return_type_offset]); - outlier_point.time_stamp = *reinterpret_cast( - &input->data[indices[walk_first_idx] + time_stamp_offset]); outlier_pcl->push_back(outlier_point); } } @@ -212,8 +208,8 @@ void RingOutlierFilterComponent::faster_filter( input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), walk_last_idx - walk_first_idx + 1)) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); @@ -222,46 +218,42 @@ void RingOutlierFilterComponent::faster_filter( output_ptr->y = p[1]; output_ptr->z = p[2]; } else { - *output_ptr = *input_ptr; + output_ptr->x = input_ptr->x; + output_ptr->y = input_ptr->y; + output_ptr->z = input_ptr->z; } const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + *reinterpret_cast(&input->data[indices[i] + input_intensity_offset]); output_ptr->intensity = intensity; + const std::uint8_t & return_type = *reinterpret_cast( + &input->data[indices[i] + input_return_type_offset]); + output_ptr->return_type = return_type; + + const std::uint8_t & channel = + *reinterpret_cast(&input->data[indices[i] + input_channel_offset]); + output_ptr->channel = channel; + output_size += output.point_step; } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i < walk_last_idx; i++) { - PointXYZIRADRT outlier_point; - - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + InputPointType outlier_point = *input_ptr; if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; outlier_point.x = p[0]; outlier_point.y = p[1]; outlier_point.z = p[2]; - } else { - outlier_point = *input_ptr; } - outlier_point.intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - outlier_point.ring = - *reinterpret_cast(&input->data[indices[i] + ring_offset]); - outlier_point.azimuth = - *reinterpret_cast(&input->data[indices[i] + azimuth_offset]); - outlier_point.distance = - *reinterpret_cast(&input->data[indices[i] + distance_offset]); - outlier_point.return_type = - *reinterpret_cast(&input->data[indices[i] + return_type_offset]); - outlier_point.time_stamp = - *reinterpret_cast(&input->data[indices[i] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } } - setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); + setUpPointCloudFormat(input, output, output_size); if (publish_outlier_pointcloud_) { PointCloud2 outlier; @@ -351,8 +343,7 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba } void RingOutlierFilterComponent::setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, - size_t num_fields) + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size) { formatted_points.data.resize(points_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform @@ -366,40 +357,40 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( formatted_points.is_bigendian = input->is_bigendian; formatted_points.is_dense = input->is_dense; - sensor_msgs::PointCloud2Modifier pcd_modifier(formatted_points); - pcd_modifier.setPointCloud2Fields( - num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, - sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + // This is a hack to get the correct fields in the output point cloud without creating the fields + // manually + sensor_msgs::msg::PointCloud2 msg_aux; + pcl::toROSMsg(pcl::PointCloud(), msg_aux); + formatted_points.fields = msg_aux.fields; } float RingOutlierFilterComponent::calculateVisibilityScore( const sensor_msgs::msg::PointCloud2 & input) { - pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); pcl::fromROSMsg(input, *input_cloud); const uint32_t vertical_bins = vertical_bins_; const uint32_t horizontal_bins = horizontal_bins_; - const float max_azimuth = max_azimuth_deg_ * 100.0; - const float min_azimuth = min_azimuth_deg_ * 100.0; + const float max_azimuth = max_azimuth_deg_ * (M_PI / 180.f); + const float min_azimuth = min_azimuth_deg_ * (M_PI / 180.f); const uint32_t horizontal_resolution = static_cast((max_azimuth - min_azimuth) / horizontal_bins); - std::vector> ring_point_clouds(vertical_bins); + std::vector> ring_point_clouds(vertical_bins); cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); // Split points into rings for (const auto & point : input_cloud->points) { - ring_point_clouds.at(point.ring).push_back(point); + ring_point_clouds.at(point.channel).push_back(point); } // Calculate frequency for each bin in each ring for (const auto & ring_points : ring_point_clouds) { if (ring_points.empty()) continue; - const uint ring_id = ring_points.front().ring; + const uint ring_id = ring_points.front().channel; std::vector frequency_in_ring(horizontal_bins, 0); for (const auto & point : ring_points.points) { diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 19920dd1ba5d7..9aba44be1065c 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -430,39 +430,56 @@ void PointCloudDataSynchronizerComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudDataSynchronizerComponent::convertToXYZICloud( +void PointCloudDataSynchronizerComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -489,9 +506,9 @@ void PointCloudDataSynchronizerComponent::cloud_callback( { std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); if (input->data.size() > 0) { - convertToXYZICloud(input, xyzi_input_ptr); + convertToXYZIRCCloud(input, xyzirc_input_ptr); } const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); @@ -500,7 +517,7 @@ void PointCloudDataSynchronizerComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -513,7 +530,7 @@ void PointCloudDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/geometry.cpp similarity index 52% rename from sensing/pointcloud_preprocessor/src/utility/utilities.cpp rename to sensing/pointcloud_preprocessor/src/utility/geometry.cpp index ec84a2467db78..e3013d05d925f 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/geometry.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,9 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/utilities.hpp" - -#include +#include "pointcloud_preprocessor/utility/geometry.hpp" namespace pointcloud_preprocessor::utils { @@ -157,91 +155,4 @@ bool point_within_cgal_polys( return false; } -bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud2 & input) -{ - using autoware_point_types::PointIndex; - using autoware_point_types::PointXYZI; - if (input.fields.size() < 4) { - return false; - } - bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); - same_layout &= field_x.name == "x"; - same_layout &= field_x.offset == offsetof(PointXYZI, x); - same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); - same_layout &= field_y.name == "y"; - same_layout &= field_y.offset == offsetof(PointXYZI, y); - same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); - same_layout &= field_z.name == "z"; - same_layout &= field_z.offset == offsetof(PointXYZI, z); - same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); - same_layout &= field_intensity.name == "intensity"; - same_layout &= field_intensity.offset == offsetof(PointXYZI, intensity); - same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_intensity.count == 1; - return same_layout; -} - -bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input) -{ - using autoware_point_types::PointIndex; - using autoware_point_types::PointXYZIRADRT; - if (input.fields.size() < 9) { - return false; - } - bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); - same_layout &= field_x.name == "x"; - same_layout &= field_x.offset == offsetof(PointXYZIRADRT, x); - same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); - same_layout &= field_y.name == "y"; - same_layout &= field_y.offset == offsetof(PointXYZIRADRT, y); - same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); - same_layout &= field_z.name == "z"; - same_layout &= field_z.offset == offsetof(PointXYZIRADRT, z); - same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); - same_layout &= field_intensity.name == "intensity"; - same_layout &= field_intensity.offset == offsetof(PointXYZIRADRT, intensity); - same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_intensity.count == 1; - const auto & field_ring = input.fields.at(static_cast(PointIndex::Ring)); - same_layout &= field_ring.name == "ring"; - same_layout &= field_ring.offset == offsetof(PointXYZIRADRT, ring); - same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; - same_layout &= field_ring.count == 1; - const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); - same_layout &= field_azimuth.name == "azimuth"; - same_layout &= field_azimuth.offset == offsetof(PointXYZIRADRT, azimuth); - same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_azimuth.count == 1; - const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); - same_layout &= field_distance.name == "distance"; - same_layout &= field_distance.offset == offsetof(PointXYZIRADRT, distance); - same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_distance.count == 1; - const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); - same_layout &= field_return_type.name == "return_type"; - same_layout &= field_return_type.offset == offsetof(PointXYZIRADRT, return_type); - same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; - same_layout &= field_return_type.count == 1; - const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); - same_layout &= field_time_stamp.name == "time_stamp"; - same_layout &= field_time_stamp.offset == offsetof(PointXYZIRADRT, time_stamp); - same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64; - same_layout &= field_time_stamp.count == 1; - return same_layout; -} - } // namespace pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/src/utility/memory.cpp b/sensing/pointcloud_preprocessor/src/utility/memory.cpp new file mode 100644 index 0000000000000..138573a2014ff --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/utility/memory.cpp @@ -0,0 +1,211 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pointcloud_preprocessor/utility/memory.hpp" + +#include + +namespace pointcloud_preprocessor::utils +{ +bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIIndex; + using autoware_point_types::PointXYZI; + if (input.fields.size() < 4) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZI, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZI, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZI, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZI, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_intensity.count == 1; + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRCIndex; + using autoware_point_types::PointXYZIRC; + if (input.fields.size() < 6) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRC, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRC, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRC, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRC, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_intensity.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRC, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Channel)); + same_layout &= field_ring.name == "channel"; + same_layout &= field_ring.offset == offsetof(PointXYZIRC, channel); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRADRTIndex; + using autoware_point_types::PointXYZIRADRT; + if (input.fields.size() < 9) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRADRT, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRADRT, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRADRT, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRADRT, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_intensity.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Ring)); + same_layout &= field_ring.name == "ring"; + same_layout &= field_ring.offset == offsetof(PointXYZIRADRT, ring); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + same_layout &= field_azimuth.name == "azimuth"; + same_layout &= field_azimuth.offset == offsetof(PointXYZIRADRT, azimuth); + same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_azimuth.count == 1; + const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); + same_layout &= field_distance.name == "distance"; + same_layout &= field_distance.offset == offsetof(PointXYZIRADRT, distance); + same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_distance.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRADRT, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + same_layout &= field_time_stamp.name == "time_stamp"; + same_layout &= field_time_stamp.offset == offsetof(PointXYZIRADRT, time_stamp); + same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64; + same_layout &= field_time_stamp.count == 1; + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRCAEDTIndex; + using autoware_point_types::PointXYZIRCAEDT; + if (input.fields.size() != 10) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRCAEDT, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRCAEDT, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRCAEDT, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRCAEDT, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_intensity.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRCAEDT, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Channel)); + same_layout &= field_ring.name == "channel"; + same_layout &= field_ring.offset == offsetof(PointXYZIRCAEDT, channel); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + same_layout &= field_azimuth.name == "azimuth"; + same_layout &= field_azimuth.offset == offsetof(PointXYZIRCAEDT, azimuth); + same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_azimuth.count == 1; + const auto & field_elevation = input.fields.at(static_cast(PointIndex::Elevation)); + same_layout &= field_elevation.name == "elevation"; + same_layout &= field_elevation.offset == offsetof(PointXYZIRCAEDT, elevation); + same_layout &= field_elevation.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_elevation.count == 1; + const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); + same_layout &= field_distance.name == "distance"; + same_layout &= field_distance.offset == offsetof(PointXYZIRCAEDT, distance); + same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_distance.count == 1; + const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + same_layout &= field_time_stamp.name == "time_stamp"; + same_layout &= field_time_stamp.offset == offsetof(PointXYZIRCAEDT, time_stamp); + same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::UINT32; + same_layout &= field_time_stamp.count == 1; + return same_layout; +} + +} // namespace pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp index 9e5d068be52f5..3b500bfb3e982 100644 --- a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,6 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. +// Note: To regenerate the ground truth (GT) for the expected undistorted point cloud values, +// set the "debug_" value to true to display the point cloud values. Then, +// replace the expected values with the newly displayed undistorted point cloud values. +// +// Also, make sure the point stamp, twist stamp, and imu stamp are not identical. +// In the current hardcoded design, timestamp of pointcloud, twist, and imu msg are listed below +// pointcloud (1 msgs, 10points): +// 10.10, 10.11, 10.12, 10.13, 10.14, 10.15, 10.16, 10.17, 10.18, 10.19 +// twist (6msgs): +// 10.095, 10.119, 10.143, 10.167, 10.191, 10.215 +// imu (6msgs): +// 10.09, 10.117, 10.144, 10.171, 10.198, 10.225 + +#include "autoware/universe_utils/math/trigonometry.hpp" #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" #include @@ -20,10 +34,13 @@ #include #include #include +#include #include #include +#include + class DistortionCorrectorTest : public ::testing::Test { protected: @@ -41,7 +58,7 @@ class DistortionCorrectorTest : public ::testing::Test // Spin the node for a while to ensure transforms are published auto start = std::chrono::steady_clock::now(); - auto timeout = std::chrono::seconds(1); + auto timeout = std::chrono::milliseconds(100); while (std::chrono::steady_clock::now() - start < timeout) { rclcpp::spin_some(node_); std::this_thread::sleep_for(std::chrono::milliseconds(10)); @@ -50,14 +67,18 @@ class DistortionCorrectorTest : public ::testing::Test void TearDown() override {} + void checkInput(int ms) { ASSERT_LT(ms, 1000) << "ms should be less than a second."; } + rclcpp::Time addMilliseconds(rclcpp::Time stamp, int ms) { + checkInput(ms); auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); return stamp + ms_in_ns; } rclcpp::Time subtractMilliseconds(rclcpp::Time stamp, int ms) { + checkInput(ms); auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); return stamp - ms_in_ns; } @@ -66,8 +87,9 @@ class DistortionCorrectorTest : public ::testing::Test const std::string & parent_frame, const std::string & child_frame, double x, double y, double z, double qx, double qy, double qz, double qw) { + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = node_->get_clock()->now(); + tf_msg.header.stamp = timestamp; tf_msg.header.frame_id = parent_frame; tf_msg.child_frame_id = child_frame; tf_msg.transform.translation.x = x; @@ -82,6 +104,7 @@ class DistortionCorrectorTest : public ::testing::Test std::vector generateStaticTransformMsg() { + // generate defined transformations return { generateTransformMsg("base_link", "lidar_top", 5.0, 5.0, 5.0, 0.683, 0.5, 0.183, 0.499), generateTransformMsg("base_link", "imu_link", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453)}; @@ -116,11 +139,13 @@ class DistortionCorrectorTest : public ::testing::Test std::vector> twist_msgs; rclcpp::Time twist_stamp = subtractMilliseconds(pointcloud_timestamp, 5); - for (int i = 0; i < 6; ++i) { - auto twist_msg = generateTwistMsg(10.0 + i * 2, 0.02 + i * 0.01, twist_stamp); + for (int i = 0; i < number_of_twist_msgs_; ++i) { + auto twist_msg = generateTwistMsg( + twist_linear_x_ + i * twist_linear_x_increment_, + twist_angular_z_ + i * twist_angular_z_increment_, twist_stamp); twist_msgs.push_back(twist_msg); - // Make sure the twist stamp is not identical to any point stamp, and imu stamp - twist_stamp = addMilliseconds(twist_stamp, 24); + + twist_stamp = addMilliseconds(twist_stamp, twist_msgs_interval_ms_); } return twist_msgs; @@ -132,19 +157,20 @@ class DistortionCorrectorTest : public ::testing::Test std::vector> imu_msgs; rclcpp::Time imu_stamp = subtractMilliseconds(pointcloud_timestamp, 10); - for (int i = 0; i < 6; ++i) { - auto imu_msg = - generateImuMsg(0.01 + i * 0.005, -0.02 + i * 0.005, 0.05 + i * 0.005, imu_stamp); + for (int i = 0; i < number_of_imu_msgs_; ++i) { + auto imu_msg = generateImuMsg( + imu_angular_x_ + i * imu_angular_x_increment_, + imu_angular_y_ + i * imu_angular_y_increment_, + imu_angular_z_ + i * imu_angular_z_increment_, imu_stamp); imu_msgs.push_back(imu_msg); - // Make sure the imu stamp is not identical to any point stamp, and twist stamp - imu_stamp = addMilliseconds(imu_stamp, 27); + imu_stamp = addMilliseconds(imu_stamp, imu_msgs_interval_ms_); } return imu_msgs; } sensor_msgs::msg::PointCloud2 generatePointCloudMsg( - bool is_generate_points, bool is_lidar_frame, rclcpp::Time stamp) + bool generate_points, bool is_lidar_frame, rclcpp::Time stamp) { sensor_msgs::msg::PointCloud2 pointcloud_msg; pointcloud_msg.header.stamp = stamp; @@ -152,87 +178,70 @@ class DistortionCorrectorTest : public ::testing::Test pointcloud_msg.height = 1; pointcloud_msg.is_dense = true; pointcloud_msg.is_bigendian = false; - pointcloud_msg.point_step = - 20; // 3 float32 fields * 4 bytes/field + 1 float64 field * 8 bytes/field - - if (is_generate_points) { - std::vector points = { - 10.0f, 0.0f, 0.0f, // point 1 - 0.0f, 10.0f, 0.0f, // point 2 - 0.0f, 0.0f, 10.0f, // point 3 - 20.0f, 0.0f, 0.0f, // point 4 - 0.0f, 20.0f, 0.0f, // point 5 - 0.0f, 0.0f, 20.0f, // point 6 - 30.0f, 0.0f, 0.0f, // point 7 - 0.0f, 30.0f, 0.0f, // point 8 - 0.0f, 0.0f, 30.0f, // point 9 - 10.0f, 10.0f, 10.0f // point 10 - }; - - size_t number_of_points = points.size() / 3; - std::vector timestamps = - generatePointTimestamps(is_generate_points, stamp, number_of_points); - - std::vector data(number_of_points * pointcloud_msg.point_step); - - for (size_t i = 0; i < number_of_points; ++i) { - std::memcpy(data.data() + i * pointcloud_msg.point_step, &points[i * 3], 3 * sizeof(float)); - std::memcpy( - data.data() + i * pointcloud_msg.point_step + 12, ×tamps[i], sizeof(double)); - } - pointcloud_msg.width = number_of_points; - pointcloud_msg.row_step = pointcloud_msg.point_step * pointcloud_msg.width; - pointcloud_msg.data = std::move(data); + if (generate_points) { + std::array points = {{ + Eigen::Vector3f(10.0f, 0.0f, 0.0f), // point 1 + Eigen::Vector3f(0.0f, 10.0f, 0.0f), // point 2 + Eigen::Vector3f(0.0f, 0.0f, 10.0f), // point 3 + Eigen::Vector3f(20.0f, 0.0f, 0.0f), // point 4 + Eigen::Vector3f(0.0f, 20.0f, 0.0f), // point 5 + Eigen::Vector3f(0.0f, 0.0f, 20.0f), // point 6 + Eigen::Vector3f(30.0f, 0.0f, 0.0f), // point 7 + Eigen::Vector3f(0.0f, 30.0f, 0.0f), // point 8 + Eigen::Vector3f(0.0f, 0.0f, 30.0f), // point 9 + Eigen::Vector3f(10.0f, 10.0f, 10.0f) // point 10 + }}; + + // Generate timestamps for the points + std::vector timestamps = generatePointTimestamps(stamp, number_of_points_); + + sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg); + modifier.setPointCloud2Fields( + 10, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1, + sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16, + "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, "elevation", 1, + sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, + "time_stamp", 1, sensor_msgs::msg::PointField::UINT32); + + modifier.resize(number_of_points_); + + sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); + + for (size_t i = 0; i < number_of_points_; ++i) { + *iter_x = points[i].x(); + *iter_y = points[i].y(); + *iter_z = points[i].z(); + *iter_t = timestamps[i]; + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_t; + } } else { pointcloud_msg.width = 0; pointcloud_msg.row_step = 0; } - sensor_msgs::msg::PointField x_field; - x_field.name = "x"; - x_field.offset = 0; - x_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - x_field.count = 1; - - sensor_msgs::msg::PointField y_field; - y_field.name = "y"; - y_field.offset = 4; - y_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - y_field.count = 1; - - sensor_msgs::msg::PointField z_field; - z_field.name = "z"; - z_field.offset = 8; - z_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - z_field.count = 1; - - sensor_msgs::msg::PointField timestamp_field; - timestamp_field.name = "time_stamp"; - timestamp_field.offset = 12; - timestamp_field.datatype = sensor_msgs::msg::PointField::FLOAT64; - timestamp_field.count = 1; - - pointcloud_msg.fields = {x_field, y_field, z_field, timestamp_field}; - return pointcloud_msg; } - std::vector generatePointTimestamps( - bool is_generate_points, rclcpp::Time pointcloud_timestamp, size_t number_of_points) + std::vector generatePointTimestamps( + rclcpp::Time pointcloud_timestamp, size_t number_of_points) { - std::vector timestamps; - if (is_generate_points) { - rclcpp::Time point_stamp = pointcloud_timestamp; - for (size_t i = 0; i < number_of_points; ++i) { - double timestamp = point_stamp.seconds(); - timestamps.push_back(timestamp); - if (i > 0) { - point_stamp = point_stamp + rclcpp::Duration::from_seconds( - 0.001); // Assuming 1ms offset for demonstration - } - } + std::vector timestamps; + rclcpp::Time global_point_stamp = pointcloud_timestamp; + for (size_t i = 0; i < number_of_points; ++i) { + std::uint32_t relative_timestamp = (global_point_stamp - pointcloud_timestamp).nanoseconds(); + timestamps.push_back(relative_timestamp); + global_point_stamp = addMilliseconds(global_point_stamp, points_interval_ms_); } + return timestamps; } @@ -240,45 +249,77 @@ class DistortionCorrectorTest : public ::testing::Test std::shared_ptr distortion_corrector_2d_; std::shared_ptr distortion_corrector_3d_; std::shared_ptr tf_broadcaster_; + + static constexpr float standard_tolerance_{1e-4}; + static constexpr float coarse_tolerance_{5e-3}; + static constexpr int number_of_twist_msgs_{6}; + static constexpr int number_of_imu_msgs_{6}; + static constexpr size_t number_of_points_{10}; + static constexpr int32_t timestamp_seconds_{10}; + static constexpr uint32_t timestamp_nanoseconds_{100000000}; + + static constexpr double twist_linear_x_{10.0}; + static constexpr double twist_angular_z_{0.02}; + static constexpr double twist_linear_x_increment_{2.0}; + static constexpr double twist_angular_z_increment_{0.01}; + + static constexpr double imu_angular_x_{0.01}; + static constexpr double imu_angular_y_{-0.02}; + static constexpr double imu_angular_z_{0.05}; + static constexpr double imu_angular_x_increment_{0.005}; + static constexpr double imu_angular_y_increment_{0.005}; + static constexpr double imu_angular_z_increment_{0.005}; + + static constexpr int points_interval_ms_{10}; + static constexpr int twist_msgs_interval_ms_{24}; + static constexpr int imu_msgs_interval_ms_{27}; + + // for debugging or regenerating the ground truth point cloud + bool debug_{false}; }; TEST_F(DistortionCorrectorTest, TestProcessTwistMessage) { - auto twist_msg = generateTwistMsg(1.0, 0.5, node_->get_clock()->now()); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp); distortion_corrector_2d_->processTwistMessage(twist_msg); - ASSERT_FALSE(distortion_corrector_2d_->twist_queue_.empty()); - EXPECT_EQ(distortion_corrector_2d_->twist_queue_.front().twist.linear.x, 1.0); - EXPECT_EQ(distortion_corrector_2d_->twist_queue_.front().twist.angular.z, 0.5); + ASSERT_FALSE(distortion_corrector_2d_->get_twist_queue().empty()); + EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x_); + EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z_); } TEST_F(DistortionCorrectorTest, TestProcessIMUMessage) { - auto imu_msg = generateImuMsg(0.5, 0.3, 0.1, node_->get_clock()->now()); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + auto imu_msg = generateImuMsg(imu_angular_x_, imu_angular_y_, imu_angular_z_, timestamp); distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); - ASSERT_FALSE(distortion_corrector_2d_->angular_velocity_queue_.empty()); - EXPECT_NEAR(distortion_corrector_2d_->angular_velocity_queue_.front().vector.z, 0.0443032, 1e-5); + ASSERT_FALSE(distortion_corrector_2d_->get_angular_velocity_queue().empty()); + EXPECT_NEAR( + distortion_corrector_2d_->get_angular_velocity_queue().front().vector.z, -0.03159, + standard_tolerance_); } TEST_F(DistortionCorrectorTest, TestIsInputValid) { + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + // input normal pointcloud without twist - sensor_msgs::msg::PointCloud2 pointcloud = - generatePointCloudMsg(true, false, node_->get_clock()->now()); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); bool result = distortion_corrector_2d_->isInputValid(pointcloud); EXPECT_FALSE(result); // input normal pointcloud with valid twist - auto twist_msg = generateTwistMsg(1.0, 0.5, node_->get_clock()->now()); + auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp); distortion_corrector_2d_->processTwistMessage(twist_msg); - pointcloud = generatePointCloudMsg(true, false, node_->get_clock()->now()); + pointcloud = generatePointCloudMsg(true, false, timestamp); result = distortion_corrector_2d_->isInputValid(pointcloud); EXPECT_TRUE(result); // input empty pointcloud - pointcloud = generatePointCloudMsg(false, false, node_->get_clock()->now()); + pointcloud = generatePointCloudMsg(false, false, timestamp); result = distortion_corrector_2d_->isInputValid(pointcloud); EXPECT_FALSE(result); } @@ -286,27 +327,27 @@ TEST_F(DistortionCorrectorTest, TestIsInputValid) TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithBaseLink) { distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed_); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed()); } TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithLidarFrame) { distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed_); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed()); } TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithMissingFrame) { distortion_corrector_2d_->setPointCloudTransform("base_link", "missing_lidar_frame"); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists_); - EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed_); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed()); } TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) { - rclcpp::Time timestamp = node_->get_clock()->now(); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); // Generate the point cloud message sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); @@ -315,23 +356,36 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) distortion_corrector_2d_->undistortPointCloud(false, pointcloud); // Verify the point cloud is not changed - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0f, 0.0f, 10.0f}, {20.0f, 0.0f, 0.0f}, - {0.0f, 20.0f, 0.0f}, {0.0f, 0.0f, 20.0f}, {30.0f, 0.0f, 0.0f}, {0.0f, 30.0f, 0.0f}, - {0.0f, 0.0f, 30.0f}, {10.0f, 10.0f, 10.0f}, - }; - - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 1e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 1e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 1e-5); + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 10.0f), Eigen::Vector3f(20.0f, 0.0f, 0.0f), + Eigen::Vector3f(0.0f, 20.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 20.0f), + Eigen::Vector3f(30.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 30.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 30.0f), Eigen::Vector3f(10.0f, 10.0f, 10.0f)}}; + + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); } } TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud) { - rclcpp::Time timestamp = node_->get_clock()->now(); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); // Generate and process multiple twist messages auto twist_msgs = generateTwistMsgs(timestamp); for (const auto & twist_msg : twist_msgs) { @@ -352,7 +406,7 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud) TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) { // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); // Generate and process multiple twist messages @@ -366,28 +420,39 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); distortion_corrector_2d_->undistortPointCloud(false, pointcloud); - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.012002f, 5.75338e-07f, 10.0f}, - {20.024f, 0.00191863f, 0.0f}, {0.0340828f, 20.0f, 0.0f}, {0.0479994f, 4.02654e-06f, 20.0f}, - {30.06f, 0.00575818f, 0.0f}, {0.0662481f, 30.0f, 0.0f}, {0.0839996f, 1.03542e-05f, 30.0f}, - {10.0931f, 10.0029f, 10.0f}, - }; + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117124f, 10.0f, 0.0f), + Eigen::Vector3f(0.26f, 0.000135182f, 10.0f), Eigen::Vector3f(20.4f, 0.0213818f, 0.0f), + Eigen::Vector3f(0.50932f, 20.0005f, 0.0f), Eigen::Vector3f(0.699999f, 0.000819721f, 20.0f), + Eigen::Vector3f(30.8599f, 0.076f, 0.0f), Eigen::Vector3f(0.947959f, 30.0016f, 0.0f), + Eigen::Vector3f(1.22f, 0.00244382f, 30.0f), Eigen::Vector3f(11.3568f, 10.0463f, 10.0f)}}; // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); } } TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) { // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); // Generate and process multiple twist messages @@ -406,28 +471,39 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); distortion_corrector_2d_->undistortPointCloud(true, pointcloud); - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0119991f, 5.75201e-07f, 10.0f}, - {20.024f, 0.0019192f, 0.0f}, {0.0321653f, 20.0f, 0.0f}, {0.0479994f, 6.32762e-06f, 20.0f}, - {30.06f, 0.00863842f, 0.0f}, {0.063369f, 30.0f, 0.0f}, {0.0839996f, 1.84079e-05f, 30.0f}, - {10.0912f, 10.0048f, 10.0f}, - }; + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.122876f, 9.99996f, 0.0f), + Eigen::Vector3f(0.26f, -0.000115049f, 10.0f), Eigen::Vector3f(20.4f, -0.0174931f, 0.0f), + Eigen::Vector3f(0.56301f, 19.9996f, 0.0f), Eigen::Vector3f(0.7f, -0.000627014f, 20.0f), + Eigen::Vector3f(30.86f, -0.052675f, 0.0f), Eigen::Vector3f(1.1004f, 29.9987f, 0.0f), + Eigen::Vector3f(1.22f, -0.00166245f, 30.0f), Eigen::Vector3f(11.4249f, 9.97293f, 10.0f)}}; // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); } } TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) { // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); // Generate and process multiple twist messages @@ -446,28 +522,44 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); distortion_corrector_2d_->undistortPointCloud(true, pointcloud); - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, -1.77636e-15f, -4.44089e-16f}, {-2.66454e-15f, 10.0f, -8.88178e-16f}, - {0.00622853f, 0.00600991f, 10.0084f}, {20.0106f, 0.010948f, 0.0157355f}, - {0.0176543f, 20.0176f, 0.0248379f}, {0.024499f, 0.0245179f, 20.0348f}, - {30.0266f, 0.0255826f, 0.0357166f}, {0.0355204f, 30.0353f, 0.0500275f}, - {0.047132f, 0.0439795f, 30.0606f}, {10.0488f, 10.046f, 10.0636f}, - }; + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, -1.77636e-15f, -4.44089e-16f), + Eigen::Vector3f(0.049989f, 10.0608f, 0.0924992f), + Eigen::Vector3f(0.106107f, 0.130237f, 10.1986f), + Eigen::Vector3f(20.1709f, 0.210011f, 0.32034f), + Eigen::Vector3f(0.220674f, 20.2734f, 0.417974f), + Eigen::Vector3f(0.274146f, 0.347043f, 20.5341f), + Eigen::Vector3f(30.3673f, 0.457564f, 0.700818f), + Eigen::Vector3f(0.418014f, 30.5259f, 0.807963f), + Eigen::Vector3f(0.464088f, 0.600081f, 30.9292f), + Eigen::Vector3f(10.5657f, 10.7121f, 11.094f)}}; // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); } } TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) { // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); // Generate and process multiple twist messages @@ -481,28 +573,39 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); distortion_corrector_3d_->undistortPointCloud(false, pointcloud); - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, {0.0f, 10.0f, 0.0f}, {0.0119991f, 0.0f, 10.0f}, - {20.024f, 0.00120042f, 0.0f}, {0.0342002f, 20.0f, 0.0f}, {0.0479994f, 2.15994e-06f, 20.0f}, - {30.06f, 0.00450349f, 0.0f}, {0.0666005f, 30.0f, 0.0f}, {0.0839996f, 7.55993e-06f, 30.0f}, - {10.0936f, 10.0024f, 10.0f}, - }; + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117f, 10.0f, 0.0f), + Eigen::Vector3f(0.26f, 9.27035e-05f, 10.0f), Eigen::Vector3f(20.4f, 0.0222176f, 0.0f), + Eigen::Vector3f(0.51f, 20.0004f, 0.0f), Eigen::Vector3f(0.7f, 0.000706573f, 20.0f), + Eigen::Vector3f(30.8599f, 0.0760946f, 0.0f), Eigen::Vector3f(0.946998f, 30.0015f, 0.0f), + Eigen::Vector3f(1.22f, 0.00234201f, 30.0f), Eigen::Vector3f(11.3569f, 10.046f, 10.0f)}}; // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); } } TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) { // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); // Generate and process multiple twist messages @@ -521,34 +624,43 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); distortion_corrector_3d_->undistortPointCloud(true, pointcloud); - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, - {0.0f, 10.0f, 0.0f}, - {0.0118491f, -0.000149993f, 10.0f}, - {20.024f, 0.00220075f, 0.000600241f}, - {0.0327002f, 20.0f, 0.000900472f}, - {0.0468023f, -0.00119623f, 20.0f}, - {30.06f, 0.0082567f, 0.00225216f}, - {0.0621003f, 30.0f, 0.00270227f}, - {0.0808503f, -0.00313673f, 30.0f}, - {10.0904f, 10.0032f, 10.0024f}, - }; + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.123015f, 9.99998f, 0.00430552f), + Eigen::Vector3f(0.266103f, -0.00895269f, 9.99992f), + Eigen::Vector3f(20.4f, -0.0176539f, -0.0193392f), + Eigen::Vector3f(0.563265f, 19.9997f, 0.035628f), + Eigen::Vector3f(0.734597f, -0.046068f, 19.9993f), + Eigen::Vector3f(30.8599f, -0.0517931f, -0.0658165f), + Eigen::Vector3f(1.0995f, 29.9989f, 0.0956997f), + Eigen::Vector3f(1.31283f, -0.113544f, 29.9977f), + Eigen::Vector3f(11.461f, 9.93096f, 10.0035f)}}; // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); } } TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) { // Generate the point cloud message - rclcpp::Time timestamp = node_->get_clock()->now(); + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); // Generate and process multiple twist messages @@ -567,26 +679,223 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top"); distortion_corrector_3d_->undistortPointCloud(true, pointcloud); - const float * data_ptr = reinterpret_cast(pointcloud.data.data()); + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + // Expected undistorted point cloud values - std::vector> expected_pointcloud = { - {10.0f, 0.0f, 0.0f}, - {-4.76837e-07f, 10.0f, 4.76837e-07f}, - {0.00572586f, 0.00616837f, 10.0086f}, - {20.0103f, 0.0117249f, 0.0149349f}, - {0.0158343f, 20.0179f, 0.024497f}, - {0.0251098f, 0.0254798f, 20.0343f}, - {30.0259f, 0.0290527f, 0.034577f}, - {0.0319824f, 30.0358f, 0.0477753f}, - {0.0478067f, 0.0460052f, 30.06f}, - {10.0462f, 10.0489f, 10.0625f}, - }; + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.046484f, 10.0622f, 0.098484f), + Eigen::Vector3f(0.107595f, 0.123767f, 10.2026f), + Eigen::Vector3f(20.1667f, 0.22465f, 0.313351f), + Eigen::Vector3f(0.201149f, 20.2784f, 0.464665f), + Eigen::Vector3f(0.290531f, 0.303489f, 20.5452f), + Eigen::Vector3f(30.3598f, 0.494116f, 0.672914f), + Eigen::Vector3f(0.375848f, 30.5336f, 0.933633f), + Eigen::Vector3f(0.510001f, 0.479651f, 30.9493f), + Eigen::Vector3f(10.5629f, 10.6855f, 11.1461f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp); + sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process a single twist message with constant linear velocity + auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp); + + distortion_corrector_2d_->processTwistMessage(twist_msg); + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud); + + distortion_corrector_3d_->processTwistMessage(twist_msg); + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud); + + // Generate expected point cloud for testing + sensor_msgs::msg::PointCloud2 expected_pointcloud_msg = + generatePointCloudMsg(true, false, timestamp); + + // Calculate expected point cloud values based on constant linear motion + double velocity = 1.0; // 1 m/s linear velocity + sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + + std::vector expected_points; + for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { + double time_offset = static_cast(*iter_t) / 1e9; + expected_points.emplace_back( + *iter_x + static_cast(velocity * time_offset), *iter_y, *iter_z); + } // Verify each point in the undistorted point cloud - for (size_t i = 0; i < expected_pointcloud.size(); ++i) { - EXPECT_NEAR(data_ptr[i * 5], expected_pointcloud[i][0], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 1], expected_pointcloud[i][1], 5e-5); - EXPECT_NEAR(data_ptr[i * 5 + 2], expected_pointcloud[i][2], 5e-5); + sensor_msgs::PointCloud2Iterator test2d_iter_x(test2d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test2d_iter_y(test2d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test2d_iter_z(test2d_pointcloud, "z"); + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; test2d_iter_x != test2d_iter_x.end(); + ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) { + oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, expected_points[i].x()); + EXPECT_FLOAT_EQ(*test2d_iter_y, expected_points[i].y()); + EXPECT_FLOAT_EQ(*test2d_iter_z, expected_points[i].z()); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } + + // Verify each point in the undistorted 2d point cloud with undistorted 3d point cloud + test2d_iter_x = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "x"); + test2d_iter_y = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "y"); + test2d_iter_z = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "z"); + sensor_msgs::PointCloud2Iterator test3d_iter_x(test3d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test3d_iter_y(test3d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test3d_iter_z(test3d_pointcloud, "z"); + + i = 0; + oss.str(""); + oss.clear(); + + oss << "Expected pointcloud:\n"; + for (; test2d_iter_x != test2d_iter_x.end(); ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, + ++test3d_iter_x, ++test3d_iter_y, ++test3d_iter_z, + ++i) { + oss << "Point " << i << " - 2D: (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")" + << " vs 3D: (" << *test3d_iter_x << ", " << *test3d_iter_y << ", " << *test3d_iter_z + << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, *test3d_iter_x); + EXPECT_FLOAT_EQ(*test2d_iter_y, *test3d_iter_y); + EXPECT_FLOAT_EQ(*test2d_iter_z, *test3d_iter_z); + } + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp); + sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process a single twist message with constant angular velocity + auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp); + + distortion_corrector_2d_->processTwistMessage(twist_msg); + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud); + + distortion_corrector_3d_->processTwistMessage(twist_msg); + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud); + + // Generate expected point cloud for testing + sensor_msgs::msg::PointCloud2 expected_pointcloud_msg = + generatePointCloudMsg(true, false, timestamp); + + // Calculate expected point cloud values based on constant rotational motion + double angular_velocity = 0.1; // 0.1 rad/s rotational velocity + sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + + std::vector expected_pointcloud; + for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { + double time_offset = static_cast(*iter_t) / 1e9; + float angle = angular_velocity * time_offset; + + // Set the quaternion for the current angle + tf2::Quaternion quaternion; + quaternion.setValue( + 0, 0, autoware::universe_utils::sin(angle * 0.5f), + autoware::universe_utils::cos(angle * 0.5f)); + + tf2::Vector3 point(*iter_x, *iter_y, *iter_z); + tf2::Vector3 rotated_point = tf2::quatRotate(quaternion, point); + expected_pointcloud.emplace_back( + static_cast(rotated_point.x()), static_cast(rotated_point.y()), + static_cast(rotated_point.z())); + } + + // Verify each point in the undistorted 2D point cloud + sensor_msgs::PointCloud2Iterator test2d_iter_x(test2d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test2d_iter_y(test2d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test2d_iter_z(test2d_pointcloud, "z"); + + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; test2d_iter_x != test2d_iter_x.end(); + ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) { + oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, expected_pointcloud[i].x()); + EXPECT_FLOAT_EQ(*test2d_iter_y, expected_pointcloud[i].y()); + EXPECT_FLOAT_EQ(*test2d_iter_z, expected_pointcloud[i].z()); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } + + // Verify each point in the undistorted 2D point cloud with undistorted 3D point cloud + test2d_iter_x = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "x"); + test2d_iter_y = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "y"); + test2d_iter_z = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "z"); + sensor_msgs::PointCloud2Iterator test3d_iter_x(test3d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test3d_iter_y(test3d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test3d_iter_z(test3d_pointcloud, "z"); + + i = 0; + oss.str(""); + oss.clear(); + + oss << "Expected pointcloud:\n"; + for (; test2d_iter_x != test2d_iter_x.end(); ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, + ++test3d_iter_x, ++test3d_iter_y, ++test3d_iter_z, + ++i) { + oss << "Point " << i << " - 2D: (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")" + << " vs 3D: (" << *test3d_iter_x << ", " << *test3d_iter_y << ", " << *test3d_iter_z + << ")\n"; + EXPECT_NEAR(*test2d_iter_x, *test3d_iter_x, coarse_tolerance_); + EXPECT_NEAR(*test2d_iter_y, *test3d_iter_y, coarse_tolerance_); + EXPECT_NEAR(*test2d_iter_z, *test3d_iter_z, coarse_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); } } diff --git a/sensing/pointcloud_preprocessor/test/test_utilities.cpp b/sensing/pointcloud_preprocessor/test/test_utilities.cpp index 68c86dfbd0505..e3075b4b11e79 100644 --- a/sensing/pointcloud_preprocessor/test/test_utilities.cpp +++ b/sensing/pointcloud_preprocessor/test/test_utilities.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include diff --git a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt index 6ec1759fc7d4d..44b668b49c87a 100644 --- a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt +++ b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt @@ -5,13 +5,22 @@ project(radar_scan_to_pointcloud2) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(PCL REQUIRED COMPONENTS common) + # Targets -ament_auto_add_library(radar_scan_to_pointcloud2_node_component SHARED - src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_scan_to_pointcloud2_node.cpp +) + +target_include_directories(${PROJECT_NAME} + SYSTEM PUBLIC ${PCL_INCLUDE_DIRS} +) +target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} ) -rclcpp_components_register_node(radar_scan_to_pointcloud2_node_component - PLUGIN "radar_scan_to_pointcloud2::RadarScanToPointcloud2Node" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_scan_to_pointcloud2::RadarScanToPointcloud2Node" EXECUTABLE radar_scan_to_pointcloud2_node ) diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index a9d4fe7ebda8f..c8c84ee3c8a4a 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -14,8 +14,8 @@ ament_cmake_auto autoware_cmake + libpcl-common pcl_conversions - pcl_ros radar_msgs rclcpp rclcpp_components diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp similarity index 95% rename from sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp rename to sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp index 8874b744bced6..30f267eebc205 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp" +#include "radar_scan_to_pointcloud2_node.hpp" #include #include @@ -87,7 +87,7 @@ sensor_msgs::msg::PointCloud2 toDopplerPointcloud2(const radar_msgs::msg::RadarS } } // namespace -namespace radar_scan_to_pointcloud2 +namespace autoware::radar_scan_to_pointcloud2 { using radar_msgs::msg::RadarReturn; using radar_msgs::msg::RadarScan; @@ -153,7 +153,7 @@ void RadarScanToPointcloud2Node::onData(const RadarScan::ConstSharedPtr radar_ms } } -} // namespace radar_scan_to_pointcloud2 +} // namespace autoware::radar_scan_to_pointcloud2 #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_scan_to_pointcloud2::RadarScanToPointcloud2Node) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_scan_to_pointcloud2::RadarScanToPointcloud2Node) diff --git a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp similarity index 86% rename from sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp rename to sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp index bb331609f606d..f758d149b184b 100644 --- a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ -#define RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ +#ifndef RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ +#define RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -24,7 +24,7 @@ #include #include -namespace radar_scan_to_pointcloud2 +namespace autoware::radar_scan_to_pointcloud2 { using radar_msgs::msg::RadarReturn; using radar_msgs::msg::RadarScan; @@ -65,6 +65,6 @@ class RadarScanToPointcloud2Node : public rclcpp::Node NodeParam node_param_{}; }; -} // namespace radar_scan_to_pointcloud2 +} // namespace autoware::radar_scan_to_pointcloud2 -#endif // RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ +#endif // RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/radar_static_pointcloud_filter/CMakeLists.txt index ecd33d3166b19..a796a0fa9734d 100644 --- a/sensing/radar_static_pointcloud_filter/CMakeLists.txt +++ b/sensing/radar_static_pointcloud_filter/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(radar_static_pointcloud_filter_node_component SHARED - src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_static_pointcloud_filter_node.cpp ) -rclcpp_components_register_node(radar_static_pointcloud_filter_node_component - PLUGIN "radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode" EXECUTABLE radar_static_pointcloud_filter_node ) diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index b1063562b1708..45c706791535c 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -20,9 +20,6 @@ radar_msgs rclcpp rclcpp_components - tf2 - tf2_geometry_msgs - tf2_ros ament_clang_format ament_lint_auto diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp similarity index 95% rename from sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp rename to sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp index 985f30c8e34b5..74c099b9bf32b 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp" +#include "radar_static_pointcloud_filter_node.hpp" #include #include @@ -75,7 +75,7 @@ geometry_msgs::msg::Vector3 compensateEgoVehicleTwist( } } // namespace -namespace radar_static_pointcloud_filter +namespace autoware::radar_static_pointcloud_filter { using nav_msgs::msg::Odometry; using radar_msgs::msg::RadarReturn; @@ -167,7 +167,8 @@ bool RadarStaticPointcloudFilterNode::isStaticPointcloud( (compensated_velocity.x < node_param_.doppler_velocity_sd); } -} // namespace radar_static_pointcloud_filter +} // namespace autoware::radar_static_pointcloud_filter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode) diff --git a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp similarity index 87% rename from sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp rename to sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp index 72a78f35251f7..eff8437eb51d2 100644 --- a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ -#define RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ +#ifndef RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ +#define RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ #include "autoware/universe_utils/ros/transform_listener.hpp" @@ -31,7 +31,7 @@ #include #include -namespace radar_static_pointcloud_filter +namespace autoware::radar_static_pointcloud_filter { using nav_msgs::msg::Odometry; using radar_msgs::msg::RadarReturn; @@ -77,6 +77,6 @@ class RadarStaticPointcloudFilterNode : public rclcpp::Node const RadarReturn & radar_return, const Odometry::ConstSharedPtr & odom_msg, geometry_msgs::msg::TransformStamped::ConstSharedPtr transform); }; -} // namespace radar_static_pointcloud_filter +} // namespace autoware::radar_static_pointcloud_filter -#endif // RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ +#endif // RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/sensing/radar_threshold_filter/CMakeLists.txt b/sensing/radar_threshold_filter/CMakeLists.txt index 3c4ae8d40d2c1..136b41cb3b37b 100644 --- a/sensing/radar_threshold_filter/CMakeLists.txt +++ b/sensing/radar_threshold_filter/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() ## Targets -ament_auto_add_library(radar_threshold_filter_node_component SHARED - src/radar_threshold_filter_node/radar_threshold_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_threshold_filter_node.cpp ) -rclcpp_components_register_node(radar_threshold_filter_node_component - PLUGIN "radar_threshold_filter::RadarThresholdFilterNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_threshold_filter::RadarThresholdFilterNode" EXECUTABLE radar_threshold_filter_node ) @@ -24,10 +24,10 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(radar_threshold_filter ${test_files}) + ament_add_ros_isolated_gtest(${PROJECT_NAME}_test ${test_files}) - target_link_libraries(radar_threshold_filter - radar_threshold_filter_node_component + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME} ) endif() diff --git a/sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp b/sensing/radar_threshold_filter/src/radar_threshold_filter_node.cpp similarity index 95% rename from sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp rename to sensing/radar_threshold_filter/src/radar_threshold_filter_node.cpp index 3f3837b678992..7a260766c2c7a 100644 --- a/sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp +++ b/sensing/radar_threshold_filter/src/radar_threshold_filter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_threshold_filter/radar_threshold_filter_node.hpp" +#include "radar_threshold_filter_node.hpp" #include @@ -20,8 +20,6 @@ #include #include -using std::placeholders::_1; - namespace { template @@ -51,10 +49,11 @@ bool isWithin(double value, double max, double min) } } // namespace -namespace radar_threshold_filter +namespace autoware::radar_threshold_filter { using radar_msgs::msg::RadarReturn; using radar_msgs::msg::RadarScan; +using std::placeholders::_1; RadarThresholdFilterNode::RadarThresholdFilterNode(const rclcpp::NodeOptions & node_options) : Node("radar_threshold_filter", node_options) @@ -156,7 +155,7 @@ bool RadarThresholdFilterNode::isWithinThreshold(const RadarReturn & radar_retur return true; } -} // namespace radar_threshold_filter +} // namespace autoware::radar_threshold_filter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_threshold_filter::RadarThresholdFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_threshold_filter::RadarThresholdFilterNode) diff --git a/sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp b/sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp similarity index 87% rename from sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp rename to sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp index ac38261dc34f1..a8a25bd972112 100644 --- a/sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp +++ b/sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ -#define RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ +#ifndef RADAR_THRESHOLD_FILTER_NODE_HPP_ +#define RADAR_THRESHOLD_FILTER_NODE_HPP_ #include @@ -24,7 +24,7 @@ #include #include -namespace radar_threshold_filter +namespace autoware::radar_threshold_filter { using radar_msgs::msg::RadarReturn; using radar_msgs::msg::RadarScan; @@ -73,6 +73,6 @@ class RadarThresholdFilterNode : public rclcpp::Node bool isWithinThreshold(const RadarReturn & radar_return); }; -} // namespace radar_threshold_filter +} // namespace autoware::radar_threshold_filter -#endif // RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ +#endif // RADAR_THRESHOLD_FILTER_NODE_HPP_ diff --git a/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp b/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp index 71a03f7d97cad..dd9063a380d69 100644 --- a/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp +++ b/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_threshold_filter/radar_threshold_filter_node.hpp" +#include "../../src/radar_threshold_filter_node.hpp" #include @@ -21,8 +21,8 @@ TEST(RadarThresholdFilter, isWithinThreshold) { rclcpp::init(0, nullptr); + using autoware::radar_threshold_filter::RadarThresholdFilterNode; using radar_msgs::msg::RadarReturn; - using radar_threshold_filter::RadarThresholdFilterNode; const double amplitude_min = -10.0; const double amplitude_max = 100.0; diff --git a/sensing/radar_tracks_noise_filter/CMakeLists.txt b/sensing/radar_tracks_noise_filter/CMakeLists.txt index a257821a90198..436c3472c8734 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/radar_tracks_noise_filter/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(radar_tracks_noise_filter_node_component SHARED - src/radar_tracks_noise_filter_node/radar_tracks_noise_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_tracks_noise_filter_node.cpp ) -rclcpp_components_register_node(radar_tracks_noise_filter_node_component - PLUGIN "radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode" EXECUTABLE radar_tracks_noise_filter_node ) @@ -22,10 +22,10 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(radar_tracks_noise_filter ${test_files}) + ament_add_ros_isolated_gtest(${PROJECT_NAME}_test ${test_files}) - target_link_libraries(radar_tracks_noise_filter - radar_tracks_noise_filter_node_component + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME} ) endif() diff --git a/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node/radar_tracks_noise_filter_node.cpp b/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp similarity index 93% rename from sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node/radar_tracks_noise_filter_node.cpp rename to sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp index 82a0a21ff6fdc..c71a95c4f59f0 100644 --- a/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node/radar_tracks_noise_filter_node.cpp +++ b/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp" +#include "radar_tracks_noise_filter_node.hpp" #include #include @@ -38,7 +38,7 @@ bool update_param( } } // namespace -namespace radar_tracks_noise_filter +namespace autoware::radar_tracks_noise_filter { using radar_msgs::msg::RadarTrack; using radar_msgs::msg::RadarTracks; @@ -116,7 +116,8 @@ bool RadarTrackCrossingNoiseFilterNode::isNoise(const RadarTrack & radar_track) } } -} // namespace radar_tracks_noise_filter +} // namespace autoware::radar_tracks_noise_filter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode) diff --git a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp b/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp similarity index 84% rename from sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp rename to sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp index 2ff4025cc64bc..9262c34167363 100644 --- a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp +++ b/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_TRACKS_NOISE_FILTER__RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ -#define RADAR_TRACKS_NOISE_FILTER__RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ +#ifndef RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ +#define RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -23,7 +23,7 @@ #include #include -namespace radar_tracks_noise_filter +namespace autoware::radar_tracks_noise_filter { using radar_msgs::msg::RadarTrack; using radar_msgs::msg::RadarTracks; @@ -62,6 +62,6 @@ class RadarTrackCrossingNoiseFilterNode : public rclcpp::Node bool isNoise(const RadarTrack & radar_track); }; -} // namespace radar_tracks_noise_filter +} // namespace autoware::radar_tracks_noise_filter -#endif // RADAR_TRACKS_NOISE_FILTER__RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ +#endif // RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ diff --git a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp index aa08260dee2e7..ea2dfc28b5a4c 100644 --- a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp +++ b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp" +#include "../../src/radar_tracks_noise_filter_node.hpp" #include #include -std::shared_ptr get_node( +std::shared_ptr get_node( float velocity_y_threshold) { rclcpp::NodeOptions node_options; @@ -28,7 +28,8 @@ std::shared_ptr ge }); auto node = - std::make_shared(node_options); + std::make_shared( + node_options); return node; } @@ -45,8 +46,8 @@ radar_msgs::msg::RadarTrack getRadarTrack(float velocity_x, float velocity_y) TEST(RadarTracksNoiseFilter, isNoise) { + using autoware::radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode; using radar_msgs::msg::RadarTrack; - using radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode; rclcpp::init(0, nullptr); { float velocity_node_threshold = 0.0; diff --git a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp index bbd8bf7ffb546..b13f6497d2dea 100644 --- a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp +++ b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp @@ -32,7 +32,7 @@ std::vector createConnectionsMap( std::vector connection_map; // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is // index in "connection_names_1" - for (const auto & name_2 : connection_names_2) { + for (const auto * const name_2 : connection_names_2) { int mapped_idx = -1; // -1 means that we cannot create a connection between some signals for (std::size_t NAME_1_IDX = 0; NAME_1_IDX < connection_names_1.size(); NAME_1_IDX++) { if (strcmp(name_2, connection_names_1[NAME_1_IDX]) == 0) { // 0 means strings are the same diff --git a/system/autoware_component_monitor/CMakeLists.txt b/system/autoware_component_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..674b079a90563 --- /dev/null +++ b/system/autoware_component_monitor/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_component_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Boost REQUIRED COMPONENTS + filesystem +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/component_monitor_node.cpp +) +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::component_monitor::ComponentMonitor" + EXECUTABLE ${PROJECT_NAME}_node +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_unit_conversions test/test_unit_conversions.cpp) + target_link_libraries(test_unit_conversions ${PROJECT_NAME}) + target_include_directories(test_unit_conversions PRIVATE src) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/system/autoware_component_monitor/README.md b/system/autoware_component_monitor/README.md new file mode 100644 index 0000000000000..c255c420c048e --- /dev/null +++ b/system/autoware_component_monitor/README.md @@ -0,0 +1,84 @@ +# autoware_component_monitor + +The `autoware_component_monitor` package allows monitoring system usage of component containers. +The composable node inside the package is attached to a component container, and it publishes CPU and memory usage of +the container. + +## Inputs / Outputs + +### Input + +None. + +### Output + +| Name | Type | Description | +| -------------------------- | -------------------------------------------------- | ---------------------- | +| `~/component_system_usage` | `autoware_internal_msgs::msg::ResourceUsageReport` | CPU, Memory usage etc. | + +## Parameters + +### Core Parameters + +{{ json_to_markdown("system/autoware_component_monitor/schema/component_monitor.schema.json") }} + +## How to use + +Add it as a composable node in your launch file: + +```xml + + + + + ... + + + + + + + + ... + + +``` + +### Quick testing + +You can test the package by running the following command: + +```bash +ros2 component load autoware_component_monitor autoware::component_monitor::ComponentMonitor -p publish_rate:=10.0 --node-namespace + +# Example usage +ros2 component load /pointcloud_container autoware_component_monitor autoware::component_monitor::ComponentMonitor -p publish_rate:=10.0 --node-namespace /pointcloud_container +``` + +## How it works + +The package uses the `top` command under the hood. +`top -b -n 1 -E k -p PID` command is run at 10 Hz to get the system usage of the process. + +- `-b` activates the batch mode. By default, `top` doesn't exit and prints to stdout periodically. Batch mode allows + exiting the program. +- `-n` number of times should `top` prints the system usage in batch mode. +- `-p` specifies the PID of the process to monitor. +- `-E k` changes the memory unit in the summary section to KiB. + +Here is a sample output: + +```text +top - 13:57:26 up 3:14, 1 user, load average: 1,09, 1,10, 1,04 +Tasks: 1 total, 0 running, 1 sleeping, 0 stopped, 0 zombie +%Cpu(s): 0,0 us, 0,8 sy, 0,0 ni, 99,2 id, 0,0 wa, 0,0 hi, 0,0 si, 0,0 st +KiB Mem : 65532208 total, 35117428 free, 17669824 used, 12744956 buff/cache +KiB Swap: 39062524 total, 39062524 free, 0 used. 45520816 avail Mem + + PID USER PR NI VIRT RES SHR S %CPU %MEM TIME+ COMMAND + 3352 meb 20 0 2905940 1,2g 39292 S 0,0 2,0 23:24.01 awesome +``` + +We get 5th, 8th fields from the last line, which are RES, %CPU respectively. diff --git a/system/autoware_component_monitor/config/component_monitor.param.yaml b/system/autoware_component_monitor/config/component_monitor.param.yaml new file mode 100644 index 0000000000000..62cf278921460 --- /dev/null +++ b/system/autoware_component_monitor/config/component_monitor.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + publish_rate: 5.0 # Hz diff --git a/system/autoware_component_monitor/launch/component_monitor.launch.xml b/system/autoware_component_monitor/launch/component_monitor.launch.xml new file mode 100644 index 0000000000000..1b2a77eddab93 --- /dev/null +++ b/system/autoware_component_monitor/launch/component_monitor.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/system/autoware_component_monitor/package.xml b/system/autoware_component_monitor/package.xml new file mode 100644 index 0000000000000..640e1f2dc2517 --- /dev/null +++ b/system/autoware_component_monitor/package.xml @@ -0,0 +1,25 @@ + + + + autoware_component_monitor + 0.0.0 + A ROS 2 package to monitor system usage of component containers. + Mehmet Emin Başoğlu + Apache-2.0 + + ament_cmake_auto + autoware_cmake + + autoware_internal_msgs + libboost-filesystem-dev + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/autoware_component_monitor/schema/component_monitor.schema.json b/system/autoware_component_monitor/schema/component_monitor.schema.json new file mode 100644 index 0000000000000..f0edf5add718e --- /dev/null +++ b/system/autoware_component_monitor/schema/component_monitor.schema.json @@ -0,0 +1,30 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for the Component Monitor node", + "type": "object", + "definitions": { + "component_monitor": { + "type": "object", + "properties": { + "publish_rate": { + "type": "number", + "default": "5.0", + "description": "Publish rate in Hz" + } + }, + "required": ["publish_rate"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/component_monitor" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/autoware_component_monitor/src/component_monitor_node.cpp b/system/autoware_component_monitor/src/component_monitor_node.cpp new file mode 100644 index 0000000000000..3c5d6b6667725 --- /dev/null +++ b/system/autoware_component_monitor/src/component_monitor_node.cpp @@ -0,0 +1,177 @@ +// Copyright 2024 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_monitor_node.hpp" + +#include "unit_conversions.hpp" + +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::component_monitor +{ +ComponentMonitor::ComponentMonitor(const rclcpp::NodeOptions & node_options) +: Node("component_monitor", node_options), publish_rate_(declare_parameter("publish_rate")) +{ + usage_pub_ = + create_publisher("~/component_system_usage", rclcpp::SensorDataQoS()); + + // Make sure top ins installed and is in path + const auto path_top = boost::process::search_path("top"); + if (path_top.empty()) { + RCLCPP_ERROR_STREAM(get_logger(), "Couldn't find 'top' in path."); + rclcpp::shutdown(); + } + + // Get the PID of the current process + int pid = getpid(); + + environment_ = boost::this_process::environment(); + environment_["LC_NUMERIC"] = "en_US.UTF-8"; + + on_timer_tick_wrapped_ = std::bind(&ComponentMonitor::on_timer_tick, this, pid); + + timer_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(publish_rate_).period(), on_timer_tick_wrapped_); +} + +void ComponentMonitor::on_timer_tick(const int pid) const +{ + if (usage_pub_->get_subscription_count() == 0) return; + + try { + auto usage_msg = pid_to_report(pid); + usage_msg.header.stamp = this->now(); + usage_msg.pid = pid; + usage_pub_->publish(usage_msg); + } catch (std::exception & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + } catch (...) { + RCLCPP_ERROR(get_logger(), "An unknown error occurred."); + } +} + +ComponentMonitor::ResourceUsageReport ComponentMonitor::pid_to_report(const pid_t & pid) const +{ + const auto std_out = run_system_command("top -b -n 1 -E k -p " + std::to_string(pid)); + + const auto fields = parse_lines_into_words(std_out); + + ResourceUsageReport report; + report.cpu_cores_utilized = std::stof(fields.back().at(8)) / 100.0f; + report.total_memory_bytes = unit_conversions::kib_to_bytes(std::stoul(fields.at(3).at(3))); + report.free_memory_bytes = unit_conversions::kib_to_bytes(std::stoul(fields.at(3).at(5))); + report.process_memory_bytes = parse_memory_res(fields.back().at(5)); + + return report; +} + +std::stringstream ComponentMonitor::run_system_command(const std::string & cmd) const +{ + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + RCLCPP_ERROR_STREAM(get_logger(), "Error setting flags on out_fd"); + } + boost::process::pipe out_pipe{out_fd[0], out_fd[1]}; + boost::process::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + RCLCPP_ERROR_STREAM(get_logger(), "Error setting flags on err_fd"); + } + boost::process::pipe err_pipe{err_fd[0], err_fd[1]}; + boost::process::ipstream is_err{std::move(err_pipe)}; + + boost::process::child c( + cmd, environment_, boost::process::std_out > is_out, boost::process::std_err > is_err); + c.wait(); + + if (c.exit_code() != 0) { + std::ostringstream os; + is_err >> os.rdbuf(); + RCLCPP_ERROR_STREAM(get_logger(), "Error running command: " << os.str()); + } + + std::stringstream sstream; + sstream << is_out.rdbuf(); + return sstream; +} + +ComponentMonitor::VecVecStr ComponentMonitor::parse_lines_into_words( + const std::stringstream & std_out) +{ + VecVecStr fields; + std::string line; + std::istringstream input{std_out.str()}; + + while (std::getline(input, line)) { + std::istringstream iss_line{line}; + std::string word; + std::vector words; + + while (iss_line >> word) { + words.push_back(word); + } + + fields.push_back(words); + } + + return fields; +} + +std::uint64_t ComponentMonitor::parse_memory_res(const std::string & mem_res) +{ + // example 1: 12.3g + // example 2: 123 (without suffix, just bytes) + static const std::unordered_map> unit_map{ + {'k', unit_conversions::kib_to_bytes}, {'m', unit_conversions::mib_to_bytes}, + {'g', unit_conversions::gib_to_bytes}, {'t', unit_conversions::tib_to_bytes}, + {'p', unit_conversions::pib_to_bytes}, {'e', unit_conversions::eib_to_bytes}}; + + if (std::isdigit(mem_res.back())) { + return std::stoull(mem_res); // Handle plain bytes without any suffix + } + + // Extract the numeric part and the unit suffix + double value = std::stod(mem_res.substr(0, mem_res.size() - 1)); + char suffix = mem_res.back(); + + // Find the appropriate function from the map + auto it = unit_map.find(suffix); + if (it != unit_map.end()) { + const auto & conversion_function = it->second; + return conversion_function(value); + } + + // Throw an exception or handle the error as needed if the suffix is not recognized + throw std::runtime_error("Unsupported unit suffix: " + std::string(1, suffix)); +} + +} // namespace autoware::component_monitor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::component_monitor::ComponentMonitor) diff --git a/system/autoware_component_monitor/src/component_monitor_node.hpp b/system/autoware_component_monitor/src/component_monitor_node.hpp new file mode 100644 index 0000000000000..70a486eb8209b --- /dev/null +++ b/system/autoware_component_monitor/src/component_monitor_node.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COMPONENT_MONITOR_NODE_HPP_ +#define COMPONENT_MONITOR_NODE_HPP_ + +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::component_monitor +{ +class ComponentMonitor : public rclcpp::Node +{ +public: + explicit ComponentMonitor(const rclcpp::NodeOptions & node_options); + +private: + using ResourceUsageReport = autoware_internal_msgs::msg::ResourceUsageReport; + using VecVecStr = std::vector>; + + const double publish_rate_; + + std::function on_timer_tick_wrapped_; + + rclcpp::Publisher::SharedPtr usage_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + boost::process::native_environment environment_; + + void on_timer_tick(int pid) const; + + /** + * @brief Get system usage of the component. + * + * @details The output of top -b -n 1 -E k -p PID` is like below: + * + * top - 13:57:26 up 3:14, 1 user, load average: 1,09, 1,10, 1,04 + * Tasks: 1 total, 0 running, 1 sleeping, 0 stopped, 0 zombie + * %Cpu(s): 0,0 us, 0,8 sy, 0,0 ni, 99,2 id, 0,0 wa, 0,0 hi, 0,0 si, 0,0 st + * KiB Mem : 65532208 total, 35117428 free, 17669824 used, 12744956 buff/cache + * KiB Swap: 39062524 total, 39062524 free, 0 used. 45520816 avail Mem + * + * PID USER PR NI VIRT RES SHR S %CPU %MEM TIME+ COMMAND + * 3352 meb 20 0 2905940 1,2g 39292 S 0,0 2,0 23:24.01 awesome + * + * We get 5th and 8th fields, which are RES, %CPU, respectively. + */ + ResourceUsageReport pid_to_report(const pid_t & pid) const; + + /** + * @brief Run a terminal command and return the standard output. + * + * @param cmd The terminal command to run + * @return The standard output of the command + */ + std::stringstream run_system_command(const std::string & cmd) const; + + /** + * @brief Parses text from a stringstream into separated words by line. + * + * @param std_out Reference to the input stringstream. + * @return Nested vector with each inner vector containing words from one line. + */ + static VecVecStr parse_lines_into_words(const std::stringstream & std_out); + + /** + * @brief Parses a memory resource string and converts it to bytes. + * + * This function handles memory size strings with suffixes to denote + * the units (e.g., "k" for kibibytes, "m" for mebibytes, etc.). + * If the string has no suffix, it is interpreted as plain bytes. + * + * @param mem_res A string representing the memory resource with a unit suffix or just bytes. + * @return uint64_t The memory size in bytes. + * + * @exception std::runtime_error Thrown if the suffix is not recognized. + */ + static std::uint64_t parse_memory_res(const std::string & mem_res); +}; + +} // namespace autoware::component_monitor + +#endif // COMPONENT_MONITOR_NODE_HPP_ diff --git a/system/autoware_component_monitor/src/unit_conversions.hpp b/system/autoware_component_monitor/src/unit_conversions.hpp new file mode 100644 index 0000000000000..c8f3fa02da519 --- /dev/null +++ b/system/autoware_component_monitor/src/unit_conversions.hpp @@ -0,0 +1,68 @@ +// Copyright 2024 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UNIT_CONVERSIONS_HPP_ +#define UNIT_CONVERSIONS_HPP_ + +#include +#include + +namespace autoware::component_monitor::unit_conversions +{ +template +std::uint64_t kib_to_bytes(T kibibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(kibibytes * 1024); +} + +template +std::uint64_t mib_to_bytes(T mebibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(mebibytes * 1024 * 1024); +} + +template +std::uint64_t gib_to_bytes(T gibibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(gibibytes * 1024ULL * 1024ULL * 1024ULL); +} + +template +std::uint64_t tib_to_bytes(T tebibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(tebibytes * 1024ULL * 1024ULL * 1024ULL * 1024ULL); +} + +template +std::uint64_t pib_to_bytes(T pebibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(pebibytes * 1024ULL * 1024ULL * 1024ULL * 1024ULL * 1024ULL); +} + +template +std::uint64_t eib_to_bytes(T exbibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast( + exbibytes * 1024ULL * 1024ULL * 1024ULL * 1024ULL * 1024ULL * 1024ULL); +} + +} // namespace autoware::component_monitor::unit_conversions + +#endif // UNIT_CONVERSIONS_HPP_ diff --git a/system/autoware_component_monitor/test/test_unit_conversions.cpp b/system/autoware_component_monitor/test/test_unit_conversions.cpp new file mode 100644 index 0000000000000..e8104ff31b80e --- /dev/null +++ b/system/autoware_component_monitor/test/test_unit_conversions.cpp @@ -0,0 +1,57 @@ +// Copyright 2024 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "unit_conversions.hpp" + +#include + +namespace autoware::component_monitor::unit_conversions +{ +TEST(UnitConversions, kib_to_bytes) +{ + EXPECT_EQ(kib_to_bytes(1), 1024U); + EXPECT_EQ(kib_to_bytes(0), 0U); + EXPECT_EQ(kib_to_bytes(10), 10240U); +} +TEST(UnitConversions, mib_to_bytes) +{ + EXPECT_EQ(mib_to_bytes(1), 1048576U); + EXPECT_EQ(mib_to_bytes(0), 0U); + EXPECT_EQ(mib_to_bytes(10), 10485760U); +} +TEST(UnitConversions, gib_to_bytes) +{ + EXPECT_EQ(gib_to_bytes(1), 1073741824U); + EXPECT_EQ(gib_to_bytes(0), 0U); + EXPECT_EQ(gib_to_bytes(10), 10737418240U); +} +TEST(UnitConversions, tib_to_bytes) +{ + EXPECT_EQ(tib_to_bytes(1), 1099511627776U); + EXPECT_EQ(tib_to_bytes(0), 0U); + EXPECT_EQ(tib_to_bytes(10), 10995116277760U); +} +TEST(UnitConversions, pib_to_bytes) +{ + EXPECT_EQ(pib_to_bytes(1), 1125899906842624U); + EXPECT_EQ(pib_to_bytes(0), 0U); + EXPECT_EQ(pib_to_bytes(10), 11258999068426240U); +} +TEST(UnitConversions, eib_to_bytes) +{ + EXPECT_EQ(eib_to_bytes(1), 1152921504606846976U); + EXPECT_EQ(eib_to_bytes(0), 0U); + EXPECT_EQ(eib_to_bytes(10), 11529215046068469760U); +} +} // namespace autoware::component_monitor::unit_conversions diff --git a/system/autoware_processing_time_checker/CMakeLists.txt b/system/autoware_processing_time_checker/CMakeLists.txt new file mode 100644 index 0000000000000..d9b3acb9abacf --- /dev/null +++ b/system/autoware_processing_time_checker/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_processing_time_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(rclcpp REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/processing_time_checker.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::processing_time_checker::ProcessingTimeChecker" + EXECUTABLE processing_time_checker_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/autoware_processing_time_checker/README.md b/system/autoware_processing_time_checker/README.md new file mode 100644 index 0000000000000..db2dd8d041e61 --- /dev/null +++ b/system/autoware_processing_time_checker/README.md @@ -0,0 +1,36 @@ +# Processing Time Checker + +## Purpose + +This node checks whether the processing time of each module is valid or not, and send a diagnostic. +NOTE: Currently, there is no validation feature, and "OK" is always assigned in the diagnostic. + +### Standalone Startup + +```bash +ros2 launch autoware_processing_time_checker processing_time_checker.launch.xml +``` + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------- | --------------------------------- | ------------------------------ | +| `/.../processing_time_ms` | `tier4_debug_msgs/Float64Stamped` | processing time of each module | + +### Output + +| Name | Type | Description | +| ----------------------------------------- | --------------------------------- | ---------------------------------- | +| `/system/processing_time_checker/metrics` | `diagnostic_msgs/DiagnosticArray` | processing time of all the modules | + +## Parameters + +{{ json_to_markdown("system/autoware_processing_time_checker/schema/processing_time_checker.schema.json") }} + +## Assumptions / Known limits + +TBD. diff --git a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml new file mode 100644 index 0000000000000..84e7d79079dae --- /dev/null +++ b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + update_rate: 10.0 + processing_time_topic_name_list: + - /control/trajectory_follower/controller_node_exe/lateral/debug/processing_time_ms + - /control/trajectory_follower/controller_node_exe/longitudinal/debug/processing_time_ms + - /control/trajectory_follower/lane_departure_checker_node/debug/processing_time_ms + - /perception/object_recognition/prediction/map_based_prediction/debug/processing_time_ms + - /perception/object_recognition/tracking/multi_object_tracker/debug/processing_time_ms + - /planning/planning_validator/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_by_lane_change/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_obstacle_avoidance/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/goal_planner/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_right/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/side_shift/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/static_obstacle_avoidance/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/total_time/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/merge_from_private/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/walkway/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/elastic_band_smoother/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/dynamic_obstacle_stop/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/obstacle_velocity_limiter/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/out_of_lane/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/processing_time_ms + - /planning/scenario_planning/velocity_smoother/debug/processing_time_ms + - /simulation/shape_estimation/debug/processing_time_ms diff --git a/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml b/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml new file mode 100644 index 0000000000000..1e9eb6a3d03e1 --- /dev/null +++ b/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/autoware_processing_time_checker/package.xml b/system/autoware_processing_time_checker/package.xml new file mode 100644 index 0000000000000..52f6fe6c2c27d --- /dev/null +++ b/system/autoware_processing_time_checker/package.xml @@ -0,0 +1,26 @@ + + + + autoware_processing_time_checker + 1.0.0 + A package to find out nodes with common names + Takayuki Murooka + Kosuke Takeuchi + + Apache 2.0 + + ament_cmake + autoware_cmake + + diagnostic_updater + rclcpp + rclcpp_components + tier4_debug_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/autoware_processing_time_checker/schema/processing_time_checker.schema.json b/system/autoware_processing_time_checker/schema/processing_time_checker.schema.json new file mode 100644 index 0000000000000..152cedb60f8ab --- /dev/null +++ b/system/autoware_processing_time_checker/schema/processing_time_checker.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Processing Time Checker", + "type": "object", + "definitions": { + "autoware_processing_time_checker": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10, + "exclusiveMinimum": 2, + "description": "The scanning and update frequency of the checker." + }, + "processing_time_topic_name_list": { + "type": "array", + "items": { + "type": "string" + }, + "description": "The topic name list of the processing time." + } + }, + "required": ["update_rate", "processing_time_topic_name_list"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/autoware_processing_time_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.cpp b/system/autoware_processing_time_checker/src/processing_time_checker.cpp new file mode 100644 index 0000000000000..fec7a16134915 --- /dev/null +++ b/system/autoware_processing_time_checker/src/processing_time_checker.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "processing_time_checker.hpp" + +#include + +#include +#include +#include + +namespace autoware::processing_time_checker +{ + +namespace +{ +std::string remove_last_name(const std::string & str) +{ + return str.substr(0, str.find_last_of("/")); +} + +std::string get_last_name(const std::string & str) +{ + return str.substr(str.find_last_of("/") + 1); +} +} // namespace + +ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_options) +: Node("processing_time_checker", node_options) +{ + const double update_rate = declare_parameter("update_rate"); + const auto processing_time_topic_name_list = + declare_parameter>("processing_time_topic_name_list"); + + for (const auto & processing_time_topic_name : processing_time_topic_name_list) { + std::optional module_name{std::nullopt}; + + // extract module name from topic name + auto tmp_topic_name = processing_time_topic_name; + for (size_t i = 0; i < 4; ++i) { // 4 is enouh for the search depth + tmp_topic_name = remove_last_name(tmp_topic_name); + const auto module_name_candidate = get_last_name(tmp_topic_name); + // clang-format off + if ( + module_name_candidate != "processing_time_ms" && module_name_candidate != "debug" && + module_name_candidate != "total_time") + { + module_name = module_name_candidate; + break; + } + // clang-format on + } + + // register module name + if (module_name) { + module_name_map_.insert_or_assign(processing_time_topic_name, *module_name); + } else { + throw std::invalid_argument("The format of the processing time topic name is not correct."); + } + } + + // create subscribers + for (const auto & processing_time_topic_name : processing_time_topic_name_list) { + const auto & module_name = module_name_map_.at(processing_time_topic_name); + + // clang-format off + processing_time_subscribers_.push_back( + create_subscription( + processing_time_topic_name, 1, + [this, &module_name]([[maybe_unused]] const Float64Stamped & msg) { + processing_time_map_.insert_or_assign(module_name, msg.data); + })); + // clang-format on + } + + diag_pub_ = create_publisher("~/metrics", 1); + + const auto period_ns = rclcpp::Rate(update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ProcessingTimeChecker::on_timer, this)); +} + +void ProcessingTimeChecker::on_timer() +{ + // create diagnostic status + DiagnosticStatus status; + status.level = status.OK; + status.name = "processing_time"; + for (const auto & processing_time_iterator : processing_time_map_) { + const auto processing_time_topic_name = processing_time_iterator.first; + const double processing_time = processing_time_iterator.second; + + // generate diagnostic status + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = processing_time_topic_name; + key_value.value = std::to_string(processing_time); + status.values.push_back(key_value); + } + + // create diagnostic array + DiagnosticArray diag_msg; + diag_msg.header.stamp = now(); + diag_msg.status.push_back(status); + + // publish + diag_pub_->publish(diag_msg); +} +} // namespace autoware::processing_time_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::processing_time_checker::ProcessingTimeChecker) diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.hpp b/system/autoware_processing_time_checker/src/processing_time_checker.hpp new file mode 100644 index 0000000000000..eba6f2c0642e6 --- /dev/null +++ b/system/autoware_processing_time_checker/src/processing_time_checker.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PROCESSING_TIME_CHECKER_HPP_ +#define PROCESSING_TIME_CHECKER_HPP_ + +#include + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" + +#include +#include +#include + +namespace autoware::processing_time_checker +{ +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using tier4_debug_msgs::msg::Float64Stamped; + +class ProcessingTimeChecker : public rclcpp::Node +{ +public: + explicit ProcessingTimeChecker(const rclcpp::NodeOptions & node_options); + +private: + void on_timer(); + + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr diag_pub_; + std::vector::SharedPtr> processing_time_subscribers_; + + // topic name - module name + std::unordered_map module_name_map_{}; + // module name - processing time + std::unordered_map processing_time_map_{}; +}; +} // namespace autoware::processing_time_checker + +#endif // PROCESSING_TIME_CHECKER_HPP_ diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index e811ffb030cd6..f20c763d643ce 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -60,6 +60,7 @@ bool L2pingService::initialize() address.sin_family = AF_INET; address.sin_port = htons(port_); address.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(socket_, (struct sockaddr *)&address, sizeof(address)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); diff --git a/system/default_ad_api/src/utils/topics.hpp b/system/default_ad_api/src/utils/topics.hpp new file mode 100644 index 0000000000000..97d91b33c9fa3 --- /dev/null +++ b/system/default_ad_api/src/utils/topics.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS__TOPICS_HPP_ +#define UTILS__TOPICS_HPP_ + +#include +#include + +#include + +namespace default_ad_api::utils +{ + +template +MsgT ignore_stamp(MsgT msg) +{ + msg.stamp = rclcpp::Time(0, 0); + return msg; +}; + +template +void notify(PubT & pub, std::optional & prev, const MsgT & curr, FuncT && ignore) +{ + if (!prev || ignore(*prev) != ignore(curr)) { + prev = curr; + pub->publish(curr); + } +} + +} // namespace default_ad_api::utils + +#endif // UTILS__TOPICS_HPP_ diff --git a/system/default_ad_api/src/vehicle_door.cpp b/system/default_ad_api/src/vehicle_door.cpp index b94921c8fd41b..e37e91bdda8e2 100644 --- a/system/default_ad_api/src/vehicle_door.cpp +++ b/system/default_ad_api/src/vehicle_door.cpp @@ -14,6 +14,8 @@ #include "vehicle_door.hpp" +#include "utils/topics.hpp" + namespace default_ad_api { @@ -24,7 +26,14 @@ VehicleDoorNode::VehicleDoorNode(const rclcpp::NodeOptions & options) group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.relay_service(cli_command_, srv_command_, group_cli_, 3.0); adaptor.relay_service(cli_layout_, srv_layout_, group_cli_, 3.0); - adaptor.relay_message(pub_status_, sub_status_); + adaptor.init_pub(pub_status_); + adaptor.init_sub(sub_status_, this, &VehicleDoorNode::on_status); +} + +void VehicleDoorNode::on_status(vehicle_interface::DoorStatus::Message::ConstSharedPtr msg) +{ + utils::notify( + pub_status_, status_, *msg, utils::ignore_stamp); } } // namespace default_ad_api diff --git a/system/default_ad_api/src/vehicle_door.hpp b/system/default_ad_api/src/vehicle_door.hpp index 92e8918afcdf8..e83e0a164d3f6 100644 --- a/system/default_ad_api/src/vehicle_door.hpp +++ b/system/default_ad_api/src/vehicle_door.hpp @@ -19,6 +19,8 @@ #include #include +#include + // This file should be included after messages. #include "utils/types.hpp" @@ -31,6 +33,7 @@ class VehicleDoorNode : public rclcpp::Node explicit VehicleDoorNode(const rclcpp::NodeOptions & options); private: + void on_status(vehicle_interface::DoorStatus::Message::ConstSharedPtr msg); rclcpp::CallbackGroup::SharedPtr group_cli_; Srv srv_command_; Srv srv_layout_; @@ -38,6 +41,7 @@ class VehicleDoorNode : public rclcpp::Node Cli cli_command_; Cli cli_layout_; Sub sub_status_; + std::optional status_; }; } // namespace default_ad_api diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp index 553c21b5ff4d7..496c95722002a 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp @@ -68,10 +68,10 @@ FileLoader::FileLoader(const PathConfig * path) } TreeData tree = TreeData::Load(path->resolved); - const auto paths = tree.optional("files").children("files"); + const auto files = tree.optional("files").children("files"); const auto edits = tree.optional("edits").children("edits"); const auto units = tree.optional("units").children("units"); - for (const auto & data : paths) create_path_config(data); + for (const auto & data : files) create_path_config(data); for (const auto & data : edits) create_edit_config(data); for (const auto & data : units) create_unit_config(data); } diff --git a/system/diagnostic_graph_aggregator/src/common/graph/data.cpp b/system/diagnostic_graph_aggregator/src/common/graph/data.cpp index ff186098c32e3..2a4aef7e39c1f 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/data.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/data.cpp @@ -35,25 +35,25 @@ TreeData::TreeData(const YAML::Node & yaml, const TreePath & path) : path_(path) TreeData::Item TreeData::required(const std::string & name) { // TODO(Takagi, Isamu): check map type. - const auto path = path_.field(name); + const auto tree_path = path_.field(name); if (!yaml_[name]) { - throw FieldNotFound(path); + throw FieldNotFound(tree_path); } const auto data = yaml_[name]; yaml_.remove(name); - return TreeData(data, path); + return TreeData(data, tree_path); } TreeData::Item TreeData::optional(const std::string & name) { // TODO(Takagi, Isamu): check map type. - const auto path = path_.field(name); + const auto tree_path = path_.field(name); if (!yaml_[name]) { - return TreeData(YAML::Node(YAML::NodeType::Undefined), path); + return TreeData(YAML::Node(YAML::NodeType::Undefined), tree_path); } const auto data = yaml_[name]; yaml_.remove(name); - return TreeData(data, path); + return TreeData(data, tree_path); } bool TreeData::is_valid() const diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp index da801ae078e6c..d9cbdbaa64400 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp @@ -84,12 +84,12 @@ void NodeUnit::initialize_status() LeafUnit::LeafUnit(const UnitLoader & unit) : BaseUnit(unit) { - const auto node = unit.data().required("node").text(); - const auto name = unit.data().required("name").text(); - const auto sep = node.empty() ? "" : ": "; + const auto diag_node = unit.data().required("node").text(); + const auto diag_name = unit.data().required("name").text(); + const auto sep = diag_node.empty() ? "" : ": "; struct_.path = unit.path(); - struct_.name = node + sep + name; + struct_.name = diag_node + sep + diag_name; status_.level = DiagnosticStatus::STALE; } diff --git a/system/dummy_infrastructure/CMakeLists.txt b/system/dummy_infrastructure/CMakeLists.txt index cfe326d7bb53b..5d3d39d02365d 100644 --- a/system/dummy_infrastructure/CMakeLists.txt +++ b/system/dummy_infrastructure/CMakeLists.txt @@ -15,6 +15,6 @@ rclcpp_components_register_node(dummy_infrastructure_node_component ament_auto_package( INSTALL_TO_SHARE - config - launch + launch + config ) diff --git a/system/dummy_infrastructure/README.md b/system/dummy_infrastructure/README.md index 2f3196189a1b3..d032bfa1a7e3d 100644 --- a/system/dummy_infrastructure/README.md +++ b/system/dummy_infrastructure/README.md @@ -29,8 +29,9 @@ ros2 run rqt_reconfigure rqt_reconfigure | Name | Type | Default Value | Explanation | | ------------------- | ------ | ------------- | ------------------------------------------------- | -| `update_rate` | int | `10` | Timer callback period [Hz] | +| `update_rate` | double | `10.0` | Timer callback period [Hz] | | `use_first_command` | bool | `true` | Consider instrument id or not | +| `use_command_state` | bool | `false` | Consider command state or not | | `instrument_id` | string | `` | Used as command id | | `approval` | bool | `false` | set approval filed to ros param | | `is_finalized` | bool | `false` | Stop at stop_line if finalization isn't completed | diff --git a/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml b/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml index 50dc36193776b..83976798d30b2 100644 --- a/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml +++ b/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: update_rate_hz: 10.0 - use_first_command: false - use_command_state: true - instrument_id: '0' - approval: true - is_finalized: true + use_first_command: true + use_command_state: false + instrument_id: "" + approval: false + is_finalized: false diff --git a/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml b/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml index 751575e343fa6..bc45abdebec70 100644 --- a/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml +++ b/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml @@ -1,3 +1,4 @@ + diff --git a/system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json b/system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json new file mode 100644 index 0000000000000..697ec01d31a64 --- /dev/null +++ b/system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Dummy Infrastructure Node", + "type": "object", + "definitions": { + "dummy_infrastructure": { + "type": "object", + "properties": { + "update_rate_hz": { + "type": "number", + "default": "10.0", + "description": "Timer callback period [Hz]" + }, + "use_first_command": { + "type": "boolean", + "default": "true", + "description": "Consider instrument id or not" + }, + "use_command_state": { + "type": "boolean", + "default": "false", + "description": "Consider command state or not" + }, + "instrument_id": { + "type": "string", + "default": "", + "description": "Used as command id" + }, + "approval": { + "type": "boolean", + "default": "false", + "description": "set approval filed to ros param" + }, + "is_finalized": { + "type": "boolean", + "default": "false", + "description": "Stop at stop_line if finalization isn't completed" + } + }, + "required": [ + "update_rate_hz", + "use_first_command", + "use_command_state", + "instrument_id", + "approval", + "is_finalized" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/dummy_infrastructure" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp index f24ce4587de0b..485d85c926268 100644 --- a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp +++ b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp @@ -89,12 +89,12 @@ DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & nod this->add_on_set_parameters_callback(std::bind(&DummyInfrastructureNode::onSetParam, this, _1)); // Parameter - node_param_.update_rate_hz = declare_parameter("update_rate_hz", 10.0); - node_param_.use_first_command = declare_parameter("use_first_command", true); - node_param_.use_command_state = declare_parameter("use_command_state", false); - node_param_.instrument_id = declare_parameter("instrument_id", ""); - node_param_.approval = declare_parameter("approval", false); - node_param_.is_finalized = declare_parameter("is_finalized", false); + node_param_.update_rate_hz = declare_parameter("update_rate_hz"); + node_param_.use_first_command = declare_parameter("use_first_command"); + node_param_.use_command_state = declare_parameter("use_command_state"); + node_param_.instrument_id = declare_parameter("instrument_id"); + node_param_.approval = declare_parameter("approval"); + node_param_.is_finalized = declare_parameter("is_finalized"); // Subscriber sub_command_array_ = create_subscription( diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml index 96dce7eb360e4..412c59e66e2c9 100644 --- a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -2,3 +2,5 @@ ros__parameters: update_rate: 10.0 add_duplicated_node_names_to_msg: true # if true, duplicated node names are added to msg + nodes_to_ignore: # List of nodes to ignore when checking for duplicates + - /rviz2 diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp index 9d806754f3c20..66e125de666b7 100644 --- a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -33,6 +33,9 @@ class DuplicatedNodeChecker : public rclcpp::Node std::unordered_set unique_names; std::vector identical_names; for (auto name : input_name_lists) { + if (nodes_to_ignore_.count(name) != 0) { + continue; + } if (unique_names.find(name) != unique_names.end()) { identical_names.push_back(name); } else { @@ -49,6 +52,7 @@ class DuplicatedNodeChecker : public rclcpp::Node diagnostic_updater::Updater updater_{this}; rclcpp::TimerBase::SharedPtr timer_; bool add_duplicated_node_names_to_msg_; + std::unordered_set nodes_to_ignore_; }; } // namespace duplicated_node_checker diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index 384c8fdc46c5f..c9bc07cffde72 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -29,6 +29,11 @@ DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_op { double update_rate = declare_parameter("update_rate"); add_duplicated_node_names_to_msg_ = declare_parameter("add_duplicated_node_names_to_msg"); + + std::vector nodes_to_ignore_vector = + declare_parameter>("nodes_to_ignore"); + nodes_to_ignore_.insert(nodes_to_ignore_vector.begin(), nodes_to_ignore_vector.end()); + updater_.setHardwareID("duplicated_node_checker"); updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 7d7deb8c4a504..f73d0df4153ce 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -86,6 +86,8 @@ class MrmHandler : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber< autoware_adapi_v1_msgs::msg::OperationModeState> sub_operation_mode_state_{this, "~/input/api/operation_mode/state"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_gear_cmd_{this, "~/input/gear"}; tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; @@ -146,7 +148,6 @@ class MrmHandler : public rclcpp::Node void handleFailedRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); - bool isDrivingBackwards(); bool isEmergency() const; bool isControlModeAutonomous(); bool isOperationModeAutonomous(); diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/mrm_handler/launch/mrm_handler.launch.xml index 7e761157956df..c99b22e10ad77 100644 --- a/system/mrm_handler/launch/mrm_handler.launch.xml +++ b/system/mrm_handler/launch/mrm_handler.launch.xml @@ -7,6 +7,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index f2531a0556a0d..0fc0cb29ecf21 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -82,28 +82,29 @@ void MrmHandler::onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg) { stamp_operation_mode_availability_ = this->now(); + operation_mode_availability_ = msg; + const bool skip_emergency_holding_check = + !param_.use_emergency_holding || is_emergency_holding_ || !isOperationModeAutonomous(); - if (!param_.use_emergency_holding) { - operation_mode_availability_ = msg; + if (skip_emergency_holding_check) { return; } - if (!is_emergency_holding_) { - if (msg->autonomous) { - stamp_autonomous_become_unavailable_.reset(); - } else { - if (!stamp_autonomous_become_unavailable_.has_value()) { - stamp_autonomous_become_unavailable_.emplace(this->now()); - } else { - const auto emergency_duration = - (this->now() - stamp_autonomous_become_unavailable_.value()).seconds(); - if (emergency_duration > param_.timeout_emergency_recovery) { - is_emergency_holding_ = true; - } - } - } + if (msg->autonomous) { + stamp_autonomous_become_unavailable_.reset(); + return; } - operation_mode_availability_ = msg; + + // If no timestamp is available, the ego autonomous mode just became unavailable and the current + // time is recorded. + stamp_autonomous_become_unavailable_ = (!stamp_autonomous_become_unavailable_.has_value()) + ? this->now() + : stamp_autonomous_become_unavailable_; + + // Check if autonomous mode unavailable time is larger than timeout threshold. + const auto emergency_duration = + (this->now() - stamp_autonomous_become_unavailable_.value()).seconds(); + is_emergency_holding_ = (emergency_duration > param_.timeout_emergency_recovery); } void MrmHandler::publishHazardCmd() @@ -130,19 +131,20 @@ void MrmHandler::publishGearCmd() { using autoware_vehicle_msgs::msg::GearCommand; GearCommand msg; - msg.stamp = this->now(); - const auto command = [&]() { - // If stopped and use_parking is not true, send the last gear command - if (isStopped()) - return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_; - return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE; - }(); - - msg.command = command; - last_gear_command_ = msg.command; + + if (isEmergency()) { + // gear command is created within mrm_handler + msg.command = + (param_.use_parking_after_stopped && isStopped()) ? GearCommand::PARK : last_gear_command_; + } else { + // use the same gear as the input gear + auto gear = sub_gear_cmd_.takeData(); + msg.command = (gear == nullptr) ? last_gear_command_ : gear->command; + last_gear_command_ = msg.command; + } + pub_gear_cmd_->publish(msg); - return; } void MrmHandler::publishMrmState() @@ -524,14 +526,6 @@ bool MrmHandler::isStopped() return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); } -bool MrmHandler::isDrivingBackwards() -{ - auto odom = sub_odom_.takeData(); - if (odom == nullptr) return false; - constexpr auto th_moving_backwards = -0.001; - return odom->twist.twist.linear.x < th_moving_backwards; -} - bool MrmHandler::isEmergency() const { return !operation_mode_availability_->autonomous || is_emergency_holding_ || diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 1daa4624ec934..ef892bd01ff50 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -270,13 +270,12 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) void AutowareErrorMonitor::loadRequiredModules(const std::string & key) { - const auto param_key = std::string("required_modules.") + key; - const uint64_t depth = 3; - const auto param_names = this->list_parameters({param_key}, depth).names; + const auto param_names = + this->list_parameters({std::string("required_modules.") + key}, depth).names; if (param_names.empty()) { - throw std::runtime_error(fmt::format("no parameter found: {}", param_key)); + throw std::runtime_error(fmt::format("no parameter found: required_modules.{}", key)); } // Load module names from parameter key diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index f12e34c8aca46..cbb37343be05e 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -198,7 +198,8 @@ int get_ata_identify(int fd, HddInfo * info) // Create a control structure hdr.interface_id = 'S'; // This must be set to 'S' hdr.dxfer_direction = SG_DXFER_FROM_DEV; // a SCSI READ command - hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + // cppcheck-suppress cstyleCast hdr.cmdp = (unsigned char *)&ata; // SCSI command to be executed hdr.dxfer_len = sizeof(data); // number of bytes to be moved in the data transfer hdr.dxferp = data; // a pointer to user memory @@ -260,7 +261,8 @@ int get_ata_smart_data(int fd, HddInfo * info, const HddDevice & device) // Create a control structure hdr.interface_id = 'S'; // This must be set to 'S' hdr.dxfer_direction = SG_DXFER_FROM_DEV; // a SCSI READ command - hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + hdr.cmd_len = sizeof(ata); // length in bytes of the SCSI command that 'cmdp' points to + // cppcheck-suppress cstyleCast hdr.cmdp = (unsigned char *)&ata; // SCSI command to be executed hdr.dxfer_len = sizeof(data); // number of bytes to be moved in the data transfer hdr.dxferp = &data; // a pointer to user memory @@ -545,6 +547,7 @@ void run(int port) addr.sin_family = AF_INET; addr.sin_port = htons(port); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/system_monitor/reader/msr_reader/msr_reader.cpp index 16f8fe9b93477..6de88d9e23276 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/system_monitor/reader/msr_reader/msr_reader.cpp @@ -114,6 +114,7 @@ void run(int port, const std::vector & list) addr.sin_family = AF_INET; addr.sin_port = htons(port); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); diff --git a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp b/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp index e78d49b55a3df..1e6325d9c452b 100644 --- a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp +++ b/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp @@ -76,6 +76,7 @@ void CPUMonitor::checkThrottling(diagnostic_updater::DiagnosticStatusWrapper & s addr.sin_family = AF_INET; addr.sin_port = htons(msr_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { stat.summary(DiagStatus::ERROR, "connect error"); diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index c57aea81d410d..50ac4363da609 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -615,6 +615,7 @@ void HddMonitor::updateHddInfoList() addr.sin_family = AF_INET; addr.sin_port = htons(hdd_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { connect_diag_.summary(DiagStatus::ERROR, "connect error"); @@ -683,8 +684,8 @@ void HddMonitor::updateHddInfoList() // Restore HDD information list try { std::istringstream iss(buf); - boost::archive::text_iarchive oa(iss); - oa >> hdd_info_list_; + boost::archive::text_iarchive ia(iss); + ia >> hdd_info_list_; } catch (const std::exception & e) { connect_diag_.summary(DiagStatus::ERROR, "recv error"); connect_diag_.add("recv", e.what()); @@ -854,6 +855,7 @@ int HddMonitor::unmountDevice(std::string & device) addr.sin_family = AF_INET; addr.sin_port = htons(hdd_reader_port_); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { RCLCPP_ERROR(get_logger(), "socket connect error. %s", strerror(errno)); diff --git a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp b/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp index 0f321efe5c311..45ce75cbe3fae 100644 --- a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp +++ b/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp @@ -200,6 +200,7 @@ void * msr_reader(void * args) addr.sin_family = AF_INET; addr.sin_port = htons(7634); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { close(sock); diff --git a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp b/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp index 4a948029a2f3a..aa5359ba1ceb6 100644 --- a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp +++ b/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp @@ -206,6 +206,7 @@ void * hdd_reader(void * args) addr.sin_family = AF_INET; addr.sin_port = htons(7635); addr.sin_addr.s_addr = htonl(INADDR_ANY); + // cppcheck-suppress cstyleCast ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { close(sock); diff --git a/vehicle/autoware_external_cmd_converter/src/node.cpp b/vehicle/autoware_external_cmd_converter/src/node.cpp index cbff8ab5ef2ce..22fc1c319a4e3 100644 --- a/vehicle/autoware_external_cmd_converter/src/node.cpp +++ b/vehicle/autoware_external_cmd_converter/src/node.cpp @@ -179,6 +179,9 @@ void ExternalCmdConverterNode::check_topic_status( { using diagnostic_msgs::msg::DiagnosticStatus; DiagnosticStatus status; + + current_gate_mode_ = gate_mode_sub_.takeData(); + if (!check_emergency_stop_topic_timeout()) { status.level = DiagnosticStatus::ERROR; status.message = "emergency stop topic is timeout"; @@ -195,6 +198,14 @@ void ExternalCmdConverterNode::check_topic_status( bool ExternalCmdConverterNode::check_emergency_stop_topic_timeout() { + if (!current_gate_mode_) { + return true; + } + + if (current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO) { + latest_emergency_stop_heartbeat_received_time_ = nullptr; + } + if (!latest_emergency_stop_heartbeat_received_time_) { return wait_for_first_topic_; } @@ -205,8 +216,6 @@ bool ExternalCmdConverterNode::check_emergency_stop_topic_timeout() bool ExternalCmdConverterNode::check_remote_topic_rate() { - current_gate_mode_ = gate_mode_sub_.takeData(); - if (!current_gate_mode_) { return true; } diff --git a/vehicle/autoware_steer_offset_estimator/Readme.md b/vehicle/autoware_steer_offset_estimator/Readme.md index 164d411e31050..2cd6bd2aabe2a 100644 --- a/vehicle/autoware_steer_offset_estimator/Readme.md +++ b/vehicle/autoware_steer_offset_estimator/Readme.md @@ -46,7 +46,7 @@ ros2 bag play --clock ## Parameters -{{ json_to_markdown("vehicle/steer_offset_estimator/schema/steer_offset_estimator.schema.json") }} +{{ json_to_markdown("vehicle/autoware_steer_offset_estimator/schema/steer_offset_estimator.schema.json") }} ## Diagnostics