diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 7c921dac7db4c..7cfb5a52a82dd 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -3,12 +3,9 @@ checkersReport constParameterReference constVariableReference -// cspell: ignore cstyle -cstyleCast funcArgNamesDifferent functionConst functionStatic -invalidPointerCast knownConditionTrueFalse missingInclude missingIncludeSystem diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 48e4a9921bb9b..7c8c123a4ade1 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -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 kyoichi.sugahara@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,36 +82,44 @@ 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_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@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/autoware_tracking_object_merger/** 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 @@ -120,23 +129,15 @@ perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com yoshi.ri@ti 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/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_multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@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/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/** 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 perception/tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp -perception/tracking_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_fine_detector/** shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp @@ -200,10 +201,10 @@ 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/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 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/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index 2fdeef2119ab8..526ffd8dcc83a 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -14,6 +14,8 @@ ament_auto_add_library(autoware_universe_utils SHARED src/geometry/geometry.cpp src/geometry/pose_deviation.cpp src/geometry/boost_polygon_utils.cpp + src/geometry/random_convex_polygon.cpp + src/geometry/gjk_2d.cpp src/math/sin_table.cpp src/math/trigonometry.cpp src/ros/msg_operation.cpp diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 2d24f84423299..d49fced367908 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -46,59 +46,92 @@ explicit TimeKeeper(Reporters... reporters); - `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. +- `void comment(const std::string & comment);` + - Adds a comment to the current function being tracked. + - `comment`: Comment to be added. + +##### 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 "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)); + time_keeper_->comment("This is a comment for func_a"); + 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)); + time_keeper_->comment("This is a comment for func_b"); + func_c(); + } + + void func_c() + { + autoware::universe_utils::ScopedTimeTrack st("func_c", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + time_keeper_->comment("This is a comment for func_c"); + } +}; int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("time_keeper_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - time_keeper->start_track("funcA"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - time_keeper->end_track("funcA"); - }; - - auto funcB = [&time_keeper, &funcA]() { - time_keeper->start_track("funcB"); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - time_keeper->end_track("funcB"); - }; - - auto funcC = [&time_keeper, &funcB]() { - time_keeper->start_track("funcC"); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - time_keeper->end_track("funcC"); - }; - - funcC(); - + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; @@ -109,38 +142,31 @@ int main(int argc, char ** argv) ```text ========================== - funcC (6000.7ms) - └── funcB (3000.44ms) - └── funcA (1000.19ms) + func_a (6.243ms) : This is a comment for func_a + └── func_b (5.116ms) : This is a comment for func_b + └── func_c (3.055ms) : This is a comment for func_c ``` - Output (`ros2 topic echo /processing_time`) ```text + --- nodes: - id: 1 - name: funcC - processing_time: 6000.659 + name: func_a + processing_time: 6.366 parent_id: 0 + comment: This is a comment for func_a - id: 2 - name: funcB - processing_time: 3000.415 + name: func_b + processing_time: 5.237 parent_id: 1 + comment: This is a comment for func_b - id: 3 - name: funcA - processing_time: 1000.181 + name: func_c + processing_time: 3.156 parent_id: 2 - --- - ``` - -- Output (`ros2 topic echo /processing_time_str --field data`) - - ```text - funcC (6000.67ms) - └── funcB (3000.42ms) - └── funcA (1000.19ms) - - --- + comment: This is a comment for func_c ``` #### `autoware::universe_utils::ScopedTimeTrack` @@ -165,94 +191,3 @@ ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); ``` - Destroys the `ScopedTimeTrack` object, ending the tracking of the function. - -##### Example - -```cpp -#include "autoware/universe_utils/system/time_keeper.hpp" - -#include - -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("scoped_time_track_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(1)); - }; - - auto funcB = [&time_keeper, &funcA]() { - autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - }; - - auto funcC = [&time_keeper, &funcB]() { - autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - }; - - funcC(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} -``` - -- Output (console) - - ```text - ========================== - funcC (6000.7ms) - └── funcB (3000.44ms) - └── funcA (1000.19ms) - ``` - -- Output (`ros2 topic echo /processing_time`) - - ```text - nodes: - - id: 1 - name: funcC - processing_time: 6000.659 - parent_id: 0 - - id: 2 - name: funcB - processing_time: 3000.415 - parent_id: 1 - - id: 3 - name: funcA - processing_time: 1000.181 - parent_id: 2 - --- - ``` - -- Output (`ros2 topic echo /processing_time_str --field data`) - - ```text - funcC (6000.67ms) - └── funcB (3000.42ms) - └── funcA (1000.19ms) - - --- - ``` diff --git a/common/autoware_universe_utils/examples/example_polling_subscriber.cpp b/common/autoware_universe_utils/examples/example_polling_subscriber.cpp new file mode 100644 index 0000000000000..d078da6df43af --- /dev/null +++ b/common/autoware_universe_utils/examples/example_polling_subscriber.cpp @@ -0,0 +1,87 @@ +// 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/ros/polling_subscriber.hpp" + +#include + +#include + +// PublisherNode class definition +class PublisherNode : public rclcpp::Node +{ +public: + PublisherNode() : Node("publisher_node") + { + publisher_ = this->create_publisher("topic", 10); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), std::bind(&PublisherNode::publish_message, this)); + } + +private: + void publish_message() + { + auto message = std_msgs::msg::String(); + message.data = "Hello from publisher node"; + publisher_->publish(message); + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +// SubscriberNode class definition +class SubscriberNode : public rclcpp::Node +{ +public: + SubscriberNode() : Node("subscriber_node") + { + subscription_ = autoware::universe_utils::InterProcessPollingSubscriber< + std_msgs::msg::String>::create_subscription(this, "topic"); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), std::bind(&SubscriberNode::timer_callback, this)); + } + +private: + void timer_callback() const + { + auto msg = subscription_->takeData(); + if (msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + } + } + + autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr + subscription_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto publisher_node = std::make_shared(); + auto subscriber_node = std::make_shared(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(publisher_node); + executor.add_node(subscriber_node); + + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp deleted file mode 100644 index 010cc58aba8ae..0000000000000 --- a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// 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 - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("scoped_time_track_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(1)); - }; - - auto funcB = [&time_keeper, &funcA]() { - autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - }; - - auto funcC = [&time_keeper, &funcB]() { - autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - }; - - funcC(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp index 3f6037e68daac..12f0b4f31e897 100644 --- a/common/autoware_universe_utils/examples/example_time_keeper.cpp +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -15,49 +15,66 @@ #include +#include + +#include #include #include +#include -int main(int argc, char ** argv) +class ExampleNode : public rclcpp::Node { - rclcpp::init(argc, argv); - auto node = std::make_shared("time_keeper_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); +public: + ExampleNode() : Node("time_keeper_example") + { + publisher_ = + create_publisher("processing_time", 1); - auto publisher = - node->create_publisher("processing_time", 10); + 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); - time_keeper->add_reporter(publisher); + timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&ExampleNode::func_a, this)); + } - auto publisher_str = node->create_publisher("processing_time_str", 10); +private: + std::shared_ptr time_keeper_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_str_; + rclcpp::TimerBase::SharedPtr timer_; - time_keeper->add_reporter(publisher_str); + 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)); + time_keeper_->comment("This is a comment for func_a"); + func_b(); + // End constructing ProcessingTimeTree. After this, the tree will be reported (publishing + // message and outputting to std::cerr) + } - auto funcA = [&time_keeper]() { - time_keeper->start_track("funcA"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - time_keeper->end_track("funcA"); - }; + void func_b() + { + autoware::universe_utils::ScopedTimeTrack st("func_b", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + time_keeper_->comment("This is a comment for func_b"); + func_c(); + } - auto funcB = [&time_keeper, &funcA]() { - time_keeper->start_track("funcB"); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - time_keeper->end_track("funcB"); - }; - - auto funcC = [&time_keeper, &funcB]() { - time_keeper->start_track("funcC"); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - time_keeper->end_track("funcC"); - }; - - funcC(); + void func_c() + { + autoware::universe_utils::ScopedTimeTrack st("func_c", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + time_keeper_->comment("This is a comment for func_c"); + } +}; +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 e6dd57c9d3fed..e6d0363846b20 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 @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-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. @@ -577,6 +577,12 @@ std::optional intersect( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); +/** + * @brief Check if 2 convex polygons intersect using the GJK algorithm + * @details much faster than boost::geometry::intersects() + */ +bool intersects_convex(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2); + } // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/gjk_2d.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/gjk_2d.hpp new file mode 100644 index 0000000000000..7c432013215c8 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/gjk_2d.hpp @@ -0,0 +1,29 @@ +// 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__GEOMETRY__GJK_2D_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GJK_2D_HPP_ + +#include "autoware/universe_utils/geometry/boost_geometry.hpp" + +namespace autoware::universe_utils::gjk +{ +/** + * @brief Check if 2 convex polygons intersect using the GJK algorithm + * @details much faster than boost::geometry::overlaps() but limited to convex polygons + */ +bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2); +} // namespace autoware::universe_utils::gjk + +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GJK_2D_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/random_convex_polygon.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/random_convex_polygon.hpp new file mode 100644 index 0000000000000..e28c4836b1a48 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/random_convex_polygon.hpp @@ -0,0 +1,29 @@ +// 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__GEOMETRY__RANDOM_CONVEX_POLYGON_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__RANDOM_CONVEX_POLYGON_HPP_ + +#include + +namespace autoware::universe_utils +{ +/// @brief generate a random convex polygon +/// @param vertices number of vertices for the desired polygon +/// @param max points will be generated in the range [-max,max] +/// @details algorithm from https://cglab.ca/~sander/misc/ConvexGeneration/convex.html +Polygon2d random_convex_polygon(const size_t vertices, const double max); +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__RANDOM_CONVEX_POLYGON_HPP_ diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp index 1b7ea0bd69951..682c6763196c7 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/polling_subscriber.hpp @@ -11,7 +11,6 @@ // 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__ROS__POLLING_SUBSCRIBER_HPP_ #define AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ @@ -20,12 +19,15 @@ #include #include #include -#include #include namespace autoware::universe_utils { +/** + * @brief Creates a SensorDataQoS profile with a single depth. + * @return rclcpp::SensorDataQoS The QoS profile with depth set to 1. + */ inline rclcpp::SensorDataQoS SingleDepthSensorQoS() { rclcpp::SensorDataQoS qos; @@ -33,132 +35,219 @@ inline rclcpp::SensorDataQoS SingleDepthSensorQoS() return qos; } -template -class InterProcessPollingSubscriber; - -template -class InterProcessPollingSubscriber::type> +namespace polling_policy { -public: - using SharedPtr = - std::shared_ptr::type>>; - static SharedPtr create_subscription( - rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) - { - return std::make_shared>(node, topic_name, qos); - } +/** + * @brief Polling policy that keeps the latest received message. + * + * This policy retains the latest received message and provides it when requested. If a new message + * is received, it overwrites the previously stored message. + * + * @tparam MessageT The message type. + */ +template +class Latest +{ private: - typename rclcpp::Subscription::SharedPtr subscriber_; - typename T::SharedPtr data_; - -public: - explicit InterProcessPollingSubscriber( - rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) + typename MessageT::ConstSharedPtr data_{nullptr}; ///< Data pointer to store the latest data + +protected: + /** + * @brief Check the QoS settings for the subscription. + * + * @param qos The QoS profile to check. + * @throws std::invalid_argument If the QoS depth is not 1. + */ + void checkQoS(const rclcpp::QoS & qos) { - auto noexec_callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - auto noexec_subscription_options = rclcpp::SubscriptionOptions(); - noexec_subscription_options.callback_group = noexec_callback_group; - - subscriber_ = node->create_subscription( - topic_name, qos, - [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, - noexec_subscription_options); if (qos.get_rmw_qos_profile().depth > 1) { throw std::invalid_argument( "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " "serialization while updateLatestData()"); } - }; - /* - * @brief take and return the latest data from DDS queue if such data existed, otherwise return - * previous taken data("stale" data) - * @note if you want to ignore "stale" data, you should use takeNewData() - * instead - */ - typename T::ConstSharedPtr takeData() - { - auto new_data = std::make_shared(); - rclcpp::MessageInfo message_info; - const bool success = subscriber_->take(*new_data, message_info); - if (success) { - data_ = new_data; - } + } - return data_; - }; +public: + /** + * @brief Retrieve the latest data. If no new data has been received, the previously received data + * + * @return typename MessageT::ConstSharedPtr The latest data. + */ + typename MessageT::ConstSharedPtr takeData(); +}; - /* - * @brief take and return the latest data from DDS queue if such data existed, otherwise return - * nullptr instead - * @note this API allows you to avoid redundant computation on the taken data which is unchanged - * since the previous cycle +/** + * @brief Polling policy that keeps the newest received message. + * + * @tparam MessageT The message type. + */ +template +class Newest +{ +protected: + /** + * @brief Check the QoS settings for the subscription. + * + * @param qos The QoS profile to check. + * @throws std::invalid_argument If the QoS depth is not 1. */ - typename T::ConstSharedPtr takeNewData() + void checkQoS(const rclcpp::QoS & qos) { - auto new_data = std::make_shared(); - rclcpp::MessageInfo message_info; - const bool success = subscriber_->take(*new_data, message_info); - if (success) { - data_ = new_data; - return data_; - } else { - return nullptr; + if (qos.get_rmw_qos_profile().depth > 1) { + throw std::invalid_argument( + "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " + "serialization while updateLatestData()"); } } + +public: + /** + * @brief Retrieve the newest data. If no new data has been received, nullptr is returned. + * + * @return typename MessageT::ConstSharedPtr The newest data. + */ + typename MessageT::ConstSharedPtr takeData(); }; -template -class InterProcessPollingSubscriber= 2)>::type> +/** + * @brief Polling policy that keeps all received messages. + * + * @tparam MessageT The message type. + */ +template +class All { +protected: + /** + * @brief Check the QoS settings for the subscription. + * + * @param qos The QoS profile to check. + */ + void checkQoS(const rclcpp::QoS &) {} + public: - using SharedPtr = - std::shared_ptr= 2)>::type>>; - static SharedPtr create_subscription( - rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{N}) - { - return std::make_shared>(node, topic_name, qos); - } + /** + * @brief Retrieve all data. + * + * @return std::vector The list of all received data. + */ + std::vector takeData(); +}; + +} // namespace polling_policy + +/** + * @brief Subscriber class that uses a specified polling policy. + * + * @tparam MessageT The message type. + * @tparam PollingPolicy The polling policy to use. + */ +template class PollingPolicy = polling_policy::Latest> +class InterProcessPollingSubscriber : public PollingPolicy +{ + friend PollingPolicy; private: - typename rclcpp::Subscription::SharedPtr subscriber_; + typename rclcpp::Subscription::SharedPtr subscriber_; ///< Subscription object public: + using SharedPtr = std::shared_ptr>; + + /** + * @brief Construct a new InterProcessPollingSubscriber object. + * + * @param node The node to attach the subscriber to. + * @param topic_name The topic name to subscribe to. + * @param qos The QoS profile to use for the subscription. + */ explicit InterProcessPollingSubscriber( - rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{N}) + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) { + this->checkQoS(qos); + auto noexec_callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto noexec_subscription_options = rclcpp::SubscriptionOptions(); noexec_subscription_options.callback_group = noexec_callback_group; - subscriber_ = node->create_subscription( + subscriber_ = node->create_subscription( topic_name, qos, - [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, + [node]([[maybe_unused]] const typename MessageT::ConstSharedPtr msg) { assert(false); }, noexec_subscription_options); - if (qos.get_rmw_qos_profile().depth < N) { - throw std::invalid_argument( - "InterProcessPollingSubscriber will be used with depth == " + std::to_string(N) + - ", which may cause inefficient serialization while updateLatestData()"); - } - }; - std::vector takeData() + } + + /** + * @brief Create a subscription. + * + * @param node The node to attach the subscriber to. + * @param topic_name The topic name to subscribe to. + * @param qos The QoS profile to use for the subscription. + * @return SharedPtr The created subscription. + */ + static SharedPtr create_subscription( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) { - std::vector data; - rclcpp::MessageInfo message_info; - for (int i = 0; i < N; ++i) { - auto datum = std::make_shared(); - if (subscriber_->take(*datum, message_info)) { - data.push_back(datum); - } else { - break; - } - } - return data; - }; + return std::make_shared>( + node, topic_name, qos); + } + + typename rclcpp::Subscription::SharedPtr subscriber() { return subscriber_; } }; +namespace polling_policy +{ + +template +typename MessageT::ConstSharedPtr Latest::takeData() +{ + auto & subscriber = + static_cast *>(this)->subscriber_; + auto new_data = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool success = subscriber->take(*new_data, message_info); + if (success) { + data_ = new_data; + } + + return data_; +} + +template +typename MessageT::ConstSharedPtr Newest::takeData() +{ + auto & subscriber = + static_cast *>(this)->subscriber_; + auto new_data = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool success = subscriber->take(*new_data, message_info); + if (success) { + return new_data; + } + return nullptr; +} + +template +std::vector All::takeData() +{ + auto & subscriber = + static_cast *>(this)->subscriber_; + std::vector data; + rclcpp::MessageInfo message_info; + for (;;) { + auto datum = std::make_shared(); + if (subscriber->take(*datum, message_info)) { + data.push_back(datum); + } else { + break; + } + } + return data; +} + +} // namespace polling_policy + } // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_ 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 index 96070f0f30b77..d846c610bf1e2 100644 --- 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 @@ -17,14 +17,11 @@ #include "autoware/universe_utils/system/stop_watch.hpp" #include -#include #include #include #include -#include - #include #include #include @@ -89,6 +86,13 @@ class ProcessingTimeNode : public std::enable_shared_from_this parent_node_{nullptr}; //!< Shared pointer to the parent node std::vector> child_nodes_; //!< Vector of shared pointers to the child nodes @@ -134,14 +139,6 @@ class TimeKeeper */ void add_reporter(rclcpp::Publisher::SharedPtr publisher); - /** - * @brief Add a reporter to publish processing times to an rclcpp publisher with - * std_msgs::msg::String - * - * @param publisher Shared pointer to the rclcpp publisher - */ - void add_reporter(rclcpp::Publisher::SharedPtr publisher); - /** * @brief Start tracking the processing time of a function * @@ -156,6 +153,13 @@ class TimeKeeper */ void end_track(const std::string & func_name); + /** + * @brief Comment the current time node + * + * @param comment Comment to be added to the current time node + */ + void comment(const std::string & comment); + private: /** * @brief Report the processing times to all registered reporters @@ -187,6 +191,11 @@ class ScopedTimeTrack */ 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 */ diff --git a/common/autoware_universe_utils/src/geometry/geometry.cpp b/common/autoware_universe_utils/src/geometry/geometry.cpp index b0f5756fc2d94..5fda8eb3f4ca4 100644 --- a/common/autoware_universe_utils/src/geometry/geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/geometry.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. @@ -14,6 +14,8 @@ #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/gjk_2d.hpp" + #include #include @@ -383,4 +385,9 @@ std::optional intersect( return intersect_point; } +bool intersects_convex(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2) +{ + return gjk::intersects(convex_polygon1, convex_polygon2); +} + } // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/geometry/gjk_2d.cpp b/common/autoware_universe_utils/src/geometry/gjk_2d.cpp new file mode 100644 index 0000000000000..86d5592fc4779 --- /dev/null +++ b/common/autoware_universe_utils/src/geometry/gjk_2d.cpp @@ -0,0 +1,150 @@ +// 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/geometry/gjk_2d.hpp" + +#include "autoware/universe_utils/geometry/boost_geometry.hpp" + +#include + +namespace autoware::universe_utils::gjk +{ + +namespace +{ +/// @brief structure with all variables updated during the GJK loop +/// @details for performance we only want to reserve their space in memory once +struct SimplexSearch +{ + // current triangle simplex + Point2d a; + Point2d b; + Point2d c; + Point2d co; // vector from C to the origin + Point2d ca; // vector from C to A + Point2d cb; // vector from C to B + Point2d ca_perpendicular; // perpendicular to CA + Point2d cb_perpendicular; // perpendicular to CB + Point2d direction; // current search direction +}; + +/// @brief calculate the dot product between 2 points +double dot_product(const Point2d & p1, const Point2d & p2) +{ + return p1.x() * p2.x() + p1.y() * p2.y(); +} + +/// @brief calculate the index of the furthest polygon vertex in the given direction +size_t furthest_vertex_idx(const Polygon2d & poly, const Point2d & direction) +{ + auto furthest_distance = dot_product(poly.outer()[0], direction); + size_t furthest_idx = 0UL; + for (auto i = 1UL; i < poly.outer().size(); ++i) { + const auto distance = dot_product(poly.outer()[i], direction); + if (distance > furthest_distance) { + furthest_distance = distance; + furthest_idx = i; + } + } + return furthest_idx; +} + +/// @brief calculate the next Minkowski difference vertex in the given direction +Point2d support_vertex(const Polygon2d & poly1, const Polygon2d & poly2, const Point2d & direction) +{ + const auto opposite_direction = Point2d(-direction.x(), -direction.y()); + const auto idx1 = furthest_vertex_idx(poly1, direction); + const auto idx2 = furthest_vertex_idx(poly2, opposite_direction); + return Point2d( + poly1.outer()[idx1].x() - poly2.outer()[idx2].x(), + poly1.outer()[idx1].y() - poly2.outer()[idx2].y()); +} + +/// @brief return true if both points are in the same direction +bool same_direction(const Point2d & p1, const Point2d & p2) +{ + return dot_product(p1, p2) > 0.0; +} + +/// @brief return the triple cross product of the given points +Point2d cross_product(const Point2d & p1, const Point2d & p2, const Point2d & p3) +{ + const auto tmp = p1.x() * p2.y() - p1.y() * p2.x(); + return Point2d(-p3.y() * tmp, p3.x() * tmp); +} + +/// @brief update the search simplex and search direction to try to surround the origin +bool update_search_simplex_and_direction(SimplexSearch & search) +{ + bool continue_search = false; + search.co.x() = -search.c.x(); + search.co.y() = -search.c.y(); + search.ca.x() = search.a.x() - search.c.x(); + search.ca.y() = search.a.y() - search.c.y(); + search.cb.x() = search.b.x() - search.c.x(); + search.cb.y() = search.b.y() - search.c.y(); + search.ca_perpendicular = cross_product(search.cb, search.ca, search.ca); + search.cb_perpendicular = cross_product(search.ca, search.cb, search.cb); + if (same_direction(search.ca_perpendicular, search.co)) { + search.b.x() = search.c.x(); + search.b.y() = search.c.y(); + search.direction.x() = search.ca_perpendicular.x(); + search.direction.y() = search.ca_perpendicular.y(); + continue_search = true; + } else if (same_direction(search.cb_perpendicular, search.co)) { + search.a.x() = search.c.x(); + search.a.y() = search.c.y(); + search.direction.x() = search.cb_perpendicular.x(); + search.direction.y() = search.cb_perpendicular.y(); + continue_search = true; + } + return continue_search; +} +} // namespace + +/// @brief return true if the two given polygons intersect +/// @details if the intersection area is 0 (e.g., only one point or one edge intersect), the return +/// value is false +bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2) +{ + if (convex_polygon1.outer().empty() || convex_polygon2.outer().empty()) { + return false; + } + if (boost::geometry::equals(convex_polygon1, convex_polygon2)) { + return true; + } + + SimplexSearch search; + search.direction = {1.0, 0.0}; + search.a = support_vertex(convex_polygon1, convex_polygon2, search.direction); + search.direction = {-search.a.x(), -search.a.y()}; + search.b = support_vertex(convex_polygon1, convex_polygon2, search.direction); + if (dot_product(search.b, search.direction) <= 0.0) { // the Minkowski difference does not cross + // the origin + return false; // no collision + } + Point2d ab = {search.b.x() - search.a.x(), search.b.y() - search.a.y()}; + Point2d ao = {-search.a.x(), -search.a.y()}; + search.direction = cross_product(ab, ao, ab); + bool continue_search = true; + while (continue_search) { + search.c = support_vertex(convex_polygon1, convex_polygon2, search.direction); + if (!same_direction(search.c, search.direction)) { // no more vertex in the search direction + return false; // no collision + } + continue_search = update_search_simplex_and_direction(search); + } + return true; +} +} // namespace autoware::universe_utils::gjk diff --git a/common/autoware_universe_utils/src/geometry/random_convex_polygon.cpp b/common/autoware_universe_utils/src/geometry/random_convex_polygon.cpp new file mode 100644 index 0000000000000..d857956806554 --- /dev/null +++ b/common/autoware_universe_utils/src/geometry/random_convex_polygon.cpp @@ -0,0 +1,109 @@ +// 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/geometry/random_convex_polygon.hpp" + +#include + +#include +#include +#include + +namespace autoware::universe_utils +{ +namespace +{ +struct VectorsWithMin +{ + std::vector vectors; + double min; +}; + +VectorsWithMin prepare_coordinate_vectors( + const size_t nb_vertices, std::uniform_real_distribution & random_double, + std::uniform_int_distribution & random_bool, std::default_random_engine & random_engine) +{ + std::vector v; + for (auto i = 0UL; i < nb_vertices; ++i) { + v.push_back(random_double(random_engine)); + } + std::sort(v.begin(), v.end()); + const auto min_v = v.front(); + const auto max_v = v.back(); + std::vector v1; + v1.push_back(min_v); + std::vector v2; + v2.push_back(min_v); + for (auto i = 1UL; i + 1 < v.size(); ++i) { + if (random_bool(random_engine) == 0) { + v1.push_back((v[i])); + } else { + v2.push_back((v[i])); + } + } + v1.push_back(max_v); + v2.push_back(max_v); + std::vector diffs; + for (auto i = 0UL; i + 1 < v1.size(); ++i) { + diffs.push_back(v1[i + 1] - v1[i]); + } + for (auto i = 0UL; i + 1 < v2.size(); ++i) { + diffs.push_back(v2[i] - v2[i + 1]); + } + VectorsWithMin vectors; + vectors.vectors = diffs; + vectors.min = min_v; + return vectors; +} +} // namespace +Polygon2d random_convex_polygon(const size_t vertices, const double max) +{ + std::random_device r; + std::default_random_engine random_engine(r()); + std::uniform_real_distribution uniform_dist(-max, max); + std::uniform_int_distribution random_bool(0, 1); + auto xs = prepare_coordinate_vectors(vertices, uniform_dist, random_bool, random_engine); + auto ys = prepare_coordinate_vectors(vertices, uniform_dist, random_bool, random_engine); + std::shuffle(ys.vectors.begin(), ys.vectors.end(), random_engine); + LinearRing2d vectors; + for (auto i = 0UL; i < xs.vectors.size(); ++i) { + vectors.emplace_back(xs.vectors[i], ys.vectors[i]); + } + std::sort(vectors.begin(), vectors.end(), [](const Point2d & p1, const Point2d & p2) { + return std::atan2(p1.y(), p1.x()) < std::atan2(p2.y(), p2.x()); + }); + auto min_x = max; + auto min_y = max; + auto x = 0.0; + auto y = 0.0; + LinearRing2d points; + for (const auto & p : vectors) { + points.emplace_back(x, y); + x += p.x(); + y += p.y(); + min_x = std::min(p.x(), min_x); + min_y = std::min(p.y(), min_y); + } + const auto shift_x = min_x - xs.min; + const auto shift_y = min_y - ys.min; + for (auto & p : points) { + p.x() += shift_x; + p.y() += shift_y; + } + Polygon2d poly; + poly.outer() = points; + boost::geometry::correct(poly); + return poly; +} +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 58bed6677227c..88038575c70f5 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -14,7 +14,8 @@ #include "autoware/universe_utils/system/time_keeper.hpp" -#include +#include + #include namespace autoware::universe_utils @@ -42,7 +43,11 @@ std::string ProcessingTimeNode::to_string() const if (!is_root) { oss << prefix << (is_last ? "└── " : "├── "); } - oss << node.name_ << " (" << node.processing_time_ << "ms)\n"; + if (!node.comment_.empty()) { + oss << node.name_ << " (" << node.processing_time_ << "ms) : " << node.comment_ << "\n"; + } else { + 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( @@ -67,8 +72,9 @@ tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const 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 = tree_msg.nodes.size() + 1; + time_node_msg.id = static_cast(tree_msg.nodes.size() + 1); time_node_msg.parent_id = parent_id; + time_node_msg.comment = node.comment_; tree_msg.nodes.emplace_back(time_node_msg); for (const auto & child : node.child_nodes_) { @@ -92,6 +98,12 @@ void ProcessingTimeNode::set_time(const double processing_time) { processing_time_ = processing_time; } + +void ProcessingTimeNode::set_comment(const std::string & comment) +{ + comment_ = comment; +} + std::string ProcessingTimeNode::get_name() const { return name_; @@ -112,15 +124,6 @@ void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr }); } -void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) -{ - reporters_.emplace_back([publisher](const std::shared_ptr & node) { - std_msgs::msg::String msg; - msg.data = node->to_string(); - publisher->publish(msg); - }); -} - void TimeKeeper::start_track(const std::string & func_name) { if (current_time_node_ == nullptr) { @@ -132,6 +135,14 @@ void TimeKeeper::start_track(const std::string & func_name) stop_watch_.tic(func_name); } +void TimeKeeper::comment(const std::string & comment) +{ + if (current_time_node_ == nullptr) { + throw std::runtime_error("You must call start_track() first, but comment() is called"); + } + current_time_node_->set_comment(comment); +} + void TimeKeeper::end_track(const std::string & func_name) { if (current_time_node_->get_name() != func_name) { diff --git a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index 194cd7d503d12..b014d12dbd985 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-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,13 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/random_convex_polygon.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include +#include +#include + #include +#include #include constexpr double epsilon = 1e-6; @@ -1829,3 +1836,147 @@ TEST(geometry, intersect) EXPECT_NEAR(result->z, 0.0, epsilon); } } + +TEST(geometry, intersectPolygon) +{ + { // 2 triangles with intersection + autoware::universe_utils::Polygon2d poly1; + autoware::universe_utils::Polygon2d poly2; + poly1.outer().emplace_back(0, 2); + poly1.outer().emplace_back(2, 2); + poly1.outer().emplace_back(2, 0); + poly2.outer().emplace_back(1, 1); + poly2.outer().emplace_back(1, 0); + poly2.outer().emplace_back(0, 1); + boost::geometry::correct(poly1); + boost::geometry::correct(poly2); + EXPECT_TRUE(autoware::universe_utils::intersects_convex(poly1, poly2)); + } + { // 2 triangles with no intersection (but they share an edge) + autoware::universe_utils::Polygon2d poly1; + autoware::universe_utils::Polygon2d poly2; + poly1.outer().emplace_back(0, 2); + poly1.outer().emplace_back(2, 2); + poly1.outer().emplace_back(0, 0); + poly2.outer().emplace_back(0, 0); + poly2.outer().emplace_back(2, 0); + poly2.outer().emplace_back(2, 2); + boost::geometry::correct(poly1); + boost::geometry::correct(poly2); + EXPECT_FALSE(autoware::universe_utils::intersects_convex(poly1, poly2)); + } + { // 2 triangles with no intersection (but they share a point) + autoware::universe_utils::Polygon2d poly1; + autoware::universe_utils::Polygon2d poly2; + poly1.outer().emplace_back(0, 2); + poly1.outer().emplace_back(2, 2); + poly1.outer().emplace_back(0, 0); + poly2.outer().emplace_back(4, 4); + poly2.outer().emplace_back(4, 2); + poly2.outer().emplace_back(2, 2); + boost::geometry::correct(poly1); + boost::geometry::correct(poly2); + EXPECT_FALSE(autoware::universe_utils::intersects_convex(poly1, poly2)); + } + { // 2 triangles sharing a point and then with very small intersection + autoware::universe_utils::Polygon2d poly1; + autoware::universe_utils::Polygon2d poly2; + poly1.outer().emplace_back(0, 0); + poly1.outer().emplace_back(2, 2); + poly1.outer().emplace_back(4, 0); + poly2.outer().emplace_back(0, 4); + poly2.outer().emplace_back(2, 2); + poly2.outer().emplace_back(4, 4); + boost::geometry::correct(poly1); + boost::geometry::correct(poly2); + EXPECT_FALSE(autoware::universe_utils::intersects_convex(poly1, poly2)); + poly1.outer()[1].y() += 1e-12; + EXPECT_TRUE(autoware::universe_utils::intersects_convex(poly1, poly2)); + } + { // 2 triangles with no intersection and no touching + autoware::universe_utils::Polygon2d poly1; + autoware::universe_utils::Polygon2d poly2; + poly1.outer().emplace_back(0, 2); + poly1.outer().emplace_back(2, 2); + poly1.outer().emplace_back(0, 0); + poly2.outer().emplace_back(4, 4); + poly2.outer().emplace_back(5, 5); + poly2.outer().emplace_back(3, 5); + boost::geometry::correct(poly1); + boost::geometry::correct(poly2); + EXPECT_FALSE(autoware::universe_utils::intersects_convex(poly1, poly2)); + } + { // triangle and quadrilateral with intersection + autoware::universe_utils::Polygon2d poly1; + autoware::universe_utils::Polygon2d poly2; + poly1.outer().emplace_back(4, 11); + poly1.outer().emplace_back(4, 5); + poly1.outer().emplace_back(9, 9); + poly2.outer().emplace_back(5, 7); + poly2.outer().emplace_back(7, 3); + poly2.outer().emplace_back(10, 2); + poly2.outer().emplace_back(12, 7); + boost::geometry::correct(poly1); + boost::geometry::correct(poly2); + EXPECT_TRUE(autoware::universe_utils::intersects_convex(poly1, poly2)); + } +} + +TEST(geometry, intersectPolygonRand) +{ + std::vector polygons; + constexpr auto polygons_nb = 500; + constexpr auto max_vertices = 10; + constexpr auto max_values = 1000; + + autoware::universe_utils::StopWatch sw; + for (auto vertices = 3UL; vertices < max_vertices; ++vertices) { + double ground_truth_intersect_ns = 0.0; + double ground_truth_no_intersect_ns = 0.0; + double gjk_intersect_ns = 0.0; + double gjk_no_intersect_ns = 0.0; + int intersect_count = 0; + polygons.clear(); + for (auto i = 0; i < polygons_nb; ++i) { + polygons.push_back(autoware::universe_utils::random_convex_polygon(vertices, max_values)); + } + for (auto i = 0UL; i < polygons.size(); ++i) { + for (auto j = 0UL; j < polygons.size(); ++j) { + sw.tic(); + const auto ground_truth = boost::geometry::intersects(polygons[i], polygons[j]); + if (ground_truth) { + ++intersect_count; + ground_truth_intersect_ns += sw.toc(); + } else { + ground_truth_no_intersect_ns += sw.toc(); + } + sw.tic(); + const auto gjk = autoware::universe_utils::intersects_convex(polygons[i], polygons[j]); + if (gjk) { + gjk_intersect_ns += sw.toc(); + } else { + gjk_no_intersect_ns += sw.toc(); + } + if (ground_truth != gjk) { + std::cout << "Failed for the 2 polygons: "; + std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j]) + << std::endl; + } + EXPECT_EQ(ground_truth, gjk); + } + } + std::printf( + "polygons_nb = %d, vertices = %ld, %d / %d pairs with intersects\n", polygons_nb, vertices, + intersect_count, polygons_nb * polygons_nb); + std::printf( + "\tIntersect:\n\t\tBoost::geometry = %2.2f ms\n\t\tGJK = %2.2f ms\n", + ground_truth_intersect_ns / 1e6, gjk_intersect_ns / 1e6); + std::printf( + "\tNo Intersect:\n\t\tBoost::geometry = %2.2f ms\n\t\tGJK = %2.2f ms\n", + ground_truth_no_intersect_ns / 1e6, gjk_no_intersect_ns / 1e6); + std::printf( + "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tGJK = %2.2f ms\n", + (ground_truth_no_intersect_ns + ground_truth_intersect_ns) / 1e6, + (gjk_no_intersect_ns + gjk_intersect_ns) / 1e6); + } +} 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/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 5ce1f0eb064a1..9a02dd4b00934 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -39,44 +39,44 @@ namespace 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); } { @@ -87,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); } @@ -95,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); } { @@ -111,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 @@ -119,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 @@ -132,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 @@ -140,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; @@ -155,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 c28678e5c872a..a8b1bb29dafa0 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -40,7 +40,9 @@ namespace 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); @@ -55,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/control/autoware_control_validator/README.md b/control/autoware_control_validator/README.md index 9c4a9be0732a5..2ace3a403c073 100644 --- a/control/autoware_control_validator/README.md +++ b/control/autoware_control_validator/README.md @@ -6,14 +6,18 @@ The `control_validator` is a module that checks the validity of the output of th ## Supported features -The following features are supported for the validation and can have thresholds set by parameters: +The following features are supported for the validation and can have thresholds set by parameters. +The listed features below does not always correspond to the latest implementation. + +| Description | Arguments | Diagnostic equation | Implemented function name | +| ---------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | :-----------------------------------------------: | ------------------------------- | +| Inverse velocity: Measured velocity has a different sign from the target velocity. | measured velocity $v$, target velocity $\hat{v}$, and threshold velocity parameter $k$ | $v \hat{v} < 0, \quad \lvert v \rvert > k$ | `checkValidVelocityDeviation()` | +| Overspeed: Measured speed exceeds target speed significantly. | measured velocity $v$, target velocity $\hat{v}$, and threshold ratio parameter $r$ | $\lvert v \rvert > (1 + r) \lvert \hat{v} \rvert$ | `checkValidVelocityDeviation()` | - **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold. ![trajectory_deviation](./image/trajectory_deviation.drawio.svg) -Other features are to be implemented. - ## Inputs/Outputs ### Inputs @@ -53,6 +57,8 @@ The following parameters can be set for the `control_validator`: The input trajectory is detected as invalid if the index exceeds the following thresholds. -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | -| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | +| Name | Type | Description | Default value | +| :----------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | +| `thresholds.max_reverse_velocity` | double | threshold velocity to valid the vehicle velocity [m/s] | WIP | +| `thresholds.max_over_velocity_ratio` | double | threshold ratio to valid the vehicle velocity [*] | WIP | diff --git a/control/autoware_control_validator/config/control_validator.param.yaml b/control/autoware_control_validator/config/control_validator.param.yaml index 12709b18b7932..fdac1d15b58b2 100644 --- a/control/autoware_control_validator/config/control_validator.param.yaml +++ b/control/autoware_control_validator/config/control_validator.param.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: - # If the number of consecutive invalid trajectory exceeds this threshold, the Diag will be set to ERROR. # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if # the next trajectory is valid.) @@ -10,3 +9,5 @@ thresholds: max_distance_deviation: 1.0 + max_reverse_velocity: 0.2 + max_over_velocity_ratio: 0.1 diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index ecb46aee123e3..515f1c4cec339 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -42,6 +42,8 @@ using nav_msgs::msg::Odometry; struct ValidationParams { double max_distance_deviation_threshold; + double max_reverse_velocity_threshold; + double max_over_velocity_ratio_threshold; }; class ControlValidator : public rclcpp::Node @@ -52,6 +54,8 @@ class ControlValidator : public rclcpp::Node void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); + bool checkValidVelocityDeviation( + const Trajectory & reference_trajectory, const Odometry & kinematics); private: void setupDiag(); @@ -60,7 +64,9 @@ class ControlValidator : public rclcpp::Node bool isDataReady(); - void validate(const Trajectory & trajectory); + void validate( + const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory, + const Odometry & kinematics); void publishPredictedTrajectory(); void publishDebugInfo(); @@ -85,6 +91,10 @@ class ControlValidator : public rclcpp::Node ControlValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds + // ego nearest index search + double ego_nearest_dist_threshold_; + double ego_nearest_yaw_threshold_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool isAllValid(const ControlValidatorStatus & status); diff --git a/control/autoware_control_validator/msg/ControlValidatorStatus.msg b/control/autoware_control_validator/msg/ControlValidatorStatus.msg index 242bede4ece89..c7dbdbb048e4e 100644 --- a/control/autoware_control_validator/msg/ControlValidatorStatus.msg +++ b/control/autoware_control_validator/msg/ControlValidatorStatus.msg @@ -2,8 +2,11 @@ builtin_interfaces/Time stamp # states bool is_valid_max_distance_deviation +bool is_valid_velocity_deviation # values float64 max_distance_deviation +float64 desired_velocity +float64 current_velocity int64 invalid_count diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 4c8ca3c831824..e5696e3c87afb 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -15,6 +15,8 @@ #include "autoware/control_validator/control_validator.hpp" #include "autoware/control_validator/utils.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include #include @@ -51,6 +53,8 @@ void ControlValidator::setupParameters() auto & p = validation_params_; const std::string t = "thresholds."; p.max_distance_deviation_threshold = declare_parameter(t + "max_distance_deviation"); + p.max_reverse_velocity_threshold = declare_parameter(t + "max_reverse_velocity"); + p.max_over_velocity_ratio_threshold = declare_parameter(t + "max_over_velocity_ratio"); } try { @@ -88,6 +92,11 @@ void ControlValidator::setupDiag() stat, validation_status_.is_valid_max_distance_deviation, "control output is deviated from trajectory"); }); + d.add(ns + "velocity_deviation", [&](auto & stat) { + setStatus( + stat, validation_status_.is_valid_velocity_deviation, + "current velocity is deviated from the desired velocity"); + }); } bool ControlValidator::isDataReady() @@ -119,7 +128,7 @@ void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr ms debug_pose_publisher_->clearMarkers(); - validate(*current_predicted_trajectory_); + validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_); diag_updater_.force_update(); @@ -142,7 +151,9 @@ void ControlValidator::publishDebugInfo() debug_pose_publisher_->publish(); } -void ControlValidator::validate(const Trajectory & predicted_trajectory) +void ControlValidator::validate( + const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory, + const Odometry & kinematics) { if (predicted_trajectory.points.size() < 2) { RCLCPP_ERROR_THROTTLE( @@ -154,6 +165,7 @@ void ControlValidator::validate(const Trajectory & predicted_trajectory) auto & s = validation_status_; s.is_valid_max_distance_deviation = checkValidMaxDistanceDeviation(predicted_trajectory); + s.is_valid_velocity_deviation = checkValidVelocityDeviation(reference_trajectory, kinematics); s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1; } @@ -170,9 +182,32 @@ bool ControlValidator::checkValidMaxDistanceDeviation(const Trajectory & predict return true; } +bool ControlValidator::checkValidVelocityDeviation( + const Trajectory & reference_trajectory, const Odometry & kinematics) +{ + const double current_vel = kinematics.twist.twist.linear.x; + if (reference_trajectory.points.size() < 2) return true; + const double desired_vel = + autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) + .longitudinal_velocity_mps; + + validation_status_.current_velocity = current_vel; + validation_status_.desired_velocity = desired_vel; + + const bool is_over_velocity = + std::abs(current_vel) > + std::abs(desired_vel) * (1.0 + validation_params_.max_over_velocity_ratio_threshold) + + validation_params_.max_reverse_velocity_threshold; + const bool is_reverse_velocity = + std::signbit(current_vel * desired_vel) && + std::abs(current_vel) > validation_params_.max_reverse_velocity_threshold; + + return !(is_over_velocity || is_reverse_velocity); +} + bool ControlValidator::isAllValid(const ControlValidatorStatus & s) { - return s.is_valid_max_distance_deviation; + return s.is_valid_max_distance_deviation && s.is_valid_velocity_deviation; } void ControlValidator::displayStatus() diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index 876797a58df25..5364938102bf6 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -69,8 +69,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Subscriber autoware::universe_utils::InterProcessPollingSubscriber sub_odom_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ - this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware::universe_utils::polling_policy::Newest> + sub_lanelet_map_bin_{this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; autoware::universe_utils::InterProcessPollingSubscriber sub_route_{ this, "~/input/route"}; autoware::universe_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 27b636f07d4ef..c079f014cfdee 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -261,7 +261,7 @@ void LaneDepartureCheckerNode::onTimer() reference_trajectory_ = sub_reference_trajectory_.takeData(); predicted_trajectory_ = sub_predicted_trajectory_.takeData(); - const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeNewData(); + const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData(); if (lanelet_map_bin_msg) { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( diff --git a/control/autoware_operation_mode_transition_manager/src/node.hpp b/control/autoware_operation_mode_transition_manager/src/node.hpp index d69d79f9640d8..d9b8eaebc4ead 100644 --- a/control/autoware_operation_mode_transition_manager/src/node.hpp +++ b/control/autoware_operation_mode_transition_manager/src/node.hpp @@ -59,7 +59,7 @@ class OperationModeTransitionManager : public rclcpp::Node void onTimer(); void publishData(); void changeControlMode(ControlModeCommandType mode); - void changeOperationMode(std::optional mode); + void changeOperationMode(std::optional request_mode); void cancelTransition(); void processTransition(); 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/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 614f1d66b9e0d..e2437e614d211 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 @@ -79,10 +79,12 @@ class ControlEvaluatorNode : public rclcpp::Node this, "~/input/acceleration"}; autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; - autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ - this, "~/input/route", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ - this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletRoute, autoware::universe_utils::polling_policy::Newest> + route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware::universe_utils::polling_policy::Newest> + vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 47706ed56cd7a..c14878a3c6f7e 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -47,7 +47,7 @@ void ControlEvaluatorNode::getRouteData() { // route { - const auto msg = route_subscriber_.takeNewData(); + const auto msg = route_subscriber_.takeData(); if (msg) { if (msg->segments.empty()) { RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); @@ -59,7 +59,7 @@ void ControlEvaluatorNode::getRouteData() // map { - const auto msg = vector_map_subscriber_.takeNewData(); + const auto msg = vector_map_subscriber_.takeData(); if (msg) { route_handler_.setMap(*msg); } 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 e15427c59ce06..05db3a261ce04 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 @@ -154,10 +154,12 @@ class PlanningEvaluatorNode : public rclcpp::Node 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_{ - this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletRoute, autoware::universe_utils::polling_policy::Newest> + route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware::universe_utils::polling_policy::Newest> + vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ this, "~/input/acceleration"}; diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index e02367407f4ae..d378d55dd2a46 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -144,7 +144,7 @@ void PlanningEvaluatorNode::getRouteData() { // route { - const auto msg = route_subscriber_.takeNewData(); + const auto msg = route_subscriber_.takeData(); if (msg) { if (msg->segments.empty()) { RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); @@ -156,7 +156,7 @@ void PlanningEvaluatorNode::getRouteData() // map { - const auto msg = vector_map_subscriber_.takeNewData(); + const auto msg = vector_map_subscriber_.takeData(); if (msg) { route_handler_.setMap(*msg); } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 63d635af4ffdf..a577fd359563c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -247,32 +247,33 @@ rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParame updateParam(parameters, "objects_count_window_seconds", p->objects_count_window_seconds); // update parameters for each object class - const auto update_object_param = [&p, ¶meters]( - const auto & semantic, const std::string & ns) { - auto & config = p->object_parameters.at(semantic); - updateParam(parameters, ns + "check_lateral_deviation", config.check_lateral_deviation); - updateParam(parameters, ns + "check_yaw_deviation", config.check_yaw_deviation); - updateParam( - parameters, ns + "check_predicted_path_deviation", config.check_predicted_path_deviation); - updateParam(parameters, ns + "check_yaw_rate", config.check_yaw_rate); - updateParam( - parameters, ns + "check_total_objects_count", config.check_total_objects_count); - updateParam( - parameters, ns + "check_average_objects_count", config.check_average_objects_count); - updateParam( - parameters, ns + "check_interval_average_objects_count", - config.check_interval_average_objects_count); - }; - const std::string ns = "target_object."; - update_object_param(ObjectClassification::MOTORCYCLE, ns + "motorcycle."); - update_object_param(ObjectClassification::CAR, ns + "car."); - update_object_param(ObjectClassification::TRUCK, ns + "truck."); - update_object_param(ObjectClassification::TRAILER, ns + "trailer."); - update_object_param(ObjectClassification::BUS, ns + "bus."); - update_object_param(ObjectClassification::PEDESTRIAN, ns + "pedestrian."); - update_object_param(ObjectClassification::BICYCLE, ns + "bicycle."); - update_object_param(ObjectClassification::UNKNOWN, ns + "unknown."); - + { + const auto update_object_param = [&p, ¶meters]( + const auto & semantic, const std::string & ns) { + auto & config = p->object_parameters.at(semantic); + updateParam(parameters, ns + "check_lateral_deviation", config.check_lateral_deviation); + updateParam(parameters, ns + "check_yaw_deviation", config.check_yaw_deviation); + updateParam( + parameters, ns + "check_predicted_path_deviation", config.check_predicted_path_deviation); + updateParam(parameters, ns + "check_yaw_rate", config.check_yaw_rate); + updateParam( + parameters, ns + "check_total_objects_count", config.check_total_objects_count); + updateParam( + parameters, ns + "check_average_objects_count", config.check_average_objects_count); + updateParam( + parameters, ns + "check_interval_average_objects_count", + config.check_interval_average_objects_count); + }; + const std::string ns = "target_object."; + update_object_param(ObjectClassification::MOTORCYCLE, ns + "motorcycle."); + update_object_param(ObjectClassification::CAR, ns + "car."); + update_object_param(ObjectClassification::TRUCK, ns + "truck."); + update_object_param(ObjectClassification::TRAILER, ns + "trailer."); + update_object_param(ObjectClassification::BUS, ns + "bus."); + update_object_param(ObjectClassification::PEDESTRIAN, ns + "pedestrian."); + update_object_param(ObjectClassification::BICYCLE, ns + "bicycle."); + update_object_param(ObjectClassification::UNKNOWN, ns + "unknown."); + } // update debug marker parameters { const std::string ns = "debug_marker."; 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 fba0144e6cee1..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 @@ -158,7 +158,7 @@ - + @@ -213,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/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml index d4a9e40f29d97..14e256cc2fd06 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -1,8 +1,8 @@ - - + + @@ -119,7 +119,7 @@ - + @@ -131,7 +131,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 5bd2c36955b55..c62ffb06ecd42 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 @@ -1,8 +1,8 @@ - - + + @@ -142,7 +142,7 @@ - + @@ -154,7 +154,7 @@ - + @@ -177,7 +177,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index c82de41a28b50..b9bb765cb1b16 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -1,8 +1,8 @@ - - + + @@ -57,7 +57,7 @@ - + @@ -69,7 +69,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 580436411a895..cc9fe78b748a4 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 @@ -35,7 +35,7 @@ - + @@ -55,7 +55,7 @@ - + @@ -89,7 +89,7 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 349472ed5f8d3..0584376a2e80a 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -13,35 +13,35 @@ ament_cmake_auto autoware_cmake + autoware_cluster_merger autoware_crosswalk_traffic_light_estimator + autoware_detection_by_tracker autoware_map_based_prediction + autoware_multi_object_tracker + autoware_object_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 - cluster_merger + autoware_raindrop_cluster_filter + autoware_tracking_object_merger compare_map_segmentation detected_object_feature_remover detected_object_validation - detection_by_tracker elevation_map_loader euclidean_cluster ground_segmentation image_projection_based_fusion image_transport_decompressor lidar_apollo_instance_segmentation - multi_object_tracker - object_merger occupancy_grid_map_outlier_filter pointcloud_preprocessor pointcloud_to_laserscan probabilistic_occupancy_grid_map - raindrop_cluster_filter shape_estimation topic_tools - tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 7dc33e0e82ea9..04d91293a7877 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -13,6 +13,7 @@ + @@ -148,7 +149,7 @@ - + @@ -159,6 +160,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/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/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_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index ebf2b6dc10878..2e4d4b321ec54 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(localization_util SHARED src/util_func.cpp + src/diagnostics_module.cpp src/smart_pose_buffer.cpp src/tree_structured_parzen_estimator.cpp src/covariance_ellipse.cpp diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp b/localization/localization_util/include/localization_util/diagnostics_module.hpp similarity index 92% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp rename to localization/localization_util/include/localization_util/diagnostics_module.hpp index 6dfea386abaf8..0ec52cfe814af 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp +++ b/localization/localization_util/include/localization_util/diagnostics_module.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_ -#define NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_ +#ifndef LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#define LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ #include @@ -57,4 +57,4 @@ void DiagnosticsModule::add_key_value(const std::string & key, const std::string template <> void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); -#endif // NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_ +#endif // LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/src/diagnostics_module.cpp b/localization/localization_util/src/diagnostics_module.cpp similarity index 98% rename from localization/ndt_scan_matcher/src/diagnostics_module.cpp rename to localization/localization_util/src/diagnostics_module.cpp index 805ee676c5e04..fb9e122a71e24 100644 --- a/localization/ndt_scan_matcher/src/diagnostics_module.cpp +++ b/localization/localization_util/src/diagnostics_module.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/diagnostics_module.hpp" +#include "localization_util/diagnostics_module.hpp" #include diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 5e4db9571c404..c2ede4da2f543 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -26,7 +26,6 @@ find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) ament_auto_add_library(${PROJECT_NAME} SHARED - src/diagnostics_module.cpp src/map_update_module.cpp src/ndt_scan_matcher_core.cpp src/particle.cpp 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 616cb108baf4a..9e2cacab4d1fc 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -119,6 +119,9 @@ # In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance temperature: 0.1 + # Scale value for adjusting the estimated covariance by a constant multiplication + scale_factor: 1.0 + 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 d6bd975c36bf3..5374e598ba68f 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 @@ -91,6 +91,7 @@ struct HyperParameters std::vector initial_pose_offset_model_x{}; std::vector initial_pose_offset_model_y{}; double temperature{}; + double scale_factor{}; } covariance_estimation{}; } covariance{}; @@ -178,6 +179,8 @@ struct HyperParameters } covariance.covariance_estimation.temperature = node->declare_parameter("covariance.covariance_estimation.temperature"); + covariance.covariance_estimation.scale_factor = + node->declare_parameter("covariance.covariance_estimation.scale_factor"); 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..ddc5e32f782f7 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 @@ -15,8 +15,8 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#include "localization_util/diagnostics_module.hpp" #include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/diagnostics_module.hpp" #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/particle.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 119c3534cab16..92df806605357 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 @@ -17,8 +17,8 @@ #define FMT_HEADER_ONLY +#include "localization_util/diagnostics_module.hpp" #include "localization_util/smart_pose_buffer.hpp" -#include "ndt_scan_matcher/diagnostics_module.hpp" #include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/map_update_module.hpp" 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 cbee3b5da72aa..81350f3d2db37 100644 --- a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json +++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json @@ -27,13 +27,21 @@ "description": "In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance", "default": 0.1, "exclusiveMinimum": 0 + }, + "scale_factor": { + "type": "number", + "description": "Scale value for adjusting the estimated covariance by a constant multiplication", + "default": 1.0, + "exclusiveMinimum": 0 } }, "required": [ "covariance_estimation_type", "initial_pose_offset_model_x", - "initial_pose_offset_model_y" + "initial_pose_offset_model_y", + "temperature", + "scale_factor" ], "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 6d34f666b997b..097fbfa838339 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -440,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); @@ -572,8 +584,10 @@ bool NDTScanMatcher::callback_sensor_points_main( CovarianceEstimationType::FIXED_VALUE) { const Eigen::Matrix2d estimated_covariance_2d = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); - const Eigen::Matrix2d estimated_covariance_2d_adj = - pclomp::adjust_diagonal_covariance(estimated_covariance_2d, ndt_result.pose, 0.0225, 0.0225); + const Eigen::Matrix2d estimated_covariance_2d_scaled = + estimated_covariance_2d * param_.covariance.covariance_estimation.scale_factor; + const Eigen::Matrix2d estimated_covariance_2d_adj = pclomp::adjust_diagonal_covariance( + estimated_covariance_2d_scaled, 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); 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/autoware_crosswalk_traffic_light_estimator/src/node.cpp b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp index 068cfc02a6aae..5d9f06c3432b5 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp @@ -209,7 +209,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( } std::vector erase_id_list; - for (auto & last_traffic_signal : last_detect_color_) { + for (const auto & last_traffic_signal : last_detect_color_) { const auto & id = last_traffic_signal.second.first.traffic_light_group_id; if (traffic_light_id_map.count(id) == 0) { diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/autoware_detection_by_tracker/CMakeLists.txt similarity index 95% rename from perception/detection_by_tracker/CMakeLists.txt rename to perception/autoware_detection_by_tracker/CMakeLists.txt index c019b2be77587..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() 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 100% rename from perception/detection_by_tracker/src/debugger/debugger.hpp rename to perception/autoware_detection_by_tracker/src/debugger/debugger.hpp 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 100% 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 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 98% 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 9c974d72cfdca..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 @@ -85,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); @@ -96,7 +96,7 @@ 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); }; diff --git a/perception/detection_by_tracker/src/tracker/tracker_handler.cpp b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp similarity index 100% rename from perception/detection_by_tracker/src/tracker/tracker_handler.cpp rename to perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp diff --git a/perception/detection_by_tracker/src/tracker/tracker_handler.hpp b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.hpp similarity index 100% rename from perception/detection_by_tracker/src/tracker/tracker_handler.hpp rename to perception/autoware_detection_by_tracker/src/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 100% rename from perception/detection_by_tracker/src/utils/utils.hpp rename to perception/autoware_detection_by_tracker/src/utils/utils.hpp 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 4f223d5c99bd6..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 diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt similarity index 97% rename from perception/multi_object_tracker/CMakeLists.txt rename to perception/autoware_multi_object_tracker/CMakeLists.txt index 370e5bb0b9161..fe4546cc9bc60 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(multi_object_tracker) +project(autoware_multi_object_tracker) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/multi_object_tracker/README.md b/perception/autoware_multi_object_tracker/README.md similarity index 57% rename from perception/multi_object_tracker/README.md rename to perception/autoware_multi_object_tracker/README.md index b5d965e1232ed..447c74d9b3450 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/autoware_multi_object_tracker/README.md @@ -65,41 +65,16 @@ Multiple inputs are pre-defined in the input channel parameters (described below ### Input Channel parameters -Available input channels are defined in [input_channels.param.yaml](config/input_channels.param.yaml). - -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------ | ------------------------------------- | -| `` | | the name of channel | -| `.topic` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | -| `.can_spawn_new_tracker` | `bool` | a switch allow to spawn a new tracker | -| `.optional.name` | `std::string` | channel name for analysis | -| `.optional.short_name` | `std::string` | short name for visualization | +{{ json_to_markdown("perception/multi_object_tracker/schema/input_channels.schema.json") }} ### Core Parameters -Node parameters are defined in [multi_object_tracker_node.param.yaml](config/multi_object_tracker_node.param.yaml) and association parameters are defined in [data_association_matrix.param.yaml](config/data_association_matrix.param.yaml). - -#### Node parameters - -| Name | Type | Description | -| --------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------- | -| `***_tracker` | string | EKF tracker name for each class | -| `world_frame_id` | double | object kinematics definition frame | -| `enable_delay_compensation` | bool | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp | -| `publish_rate` | double | Timer frequency to output with delay compensation | -| `publish_processing_time` | bool | enable to publish debug message of process time information | -| `publish_tentative_objects` | bool | enable to publish tentative tracked objects, which have lower confidence | -| `publish_debug_markers` | bool | enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection | - -#### Association parameters - -| Name | Type | Description | -| ------------------- | ------ | ------------------------------------------- | -| `can_assign_matrix` | double | Assignment table for data association | -| `max_dist_matrix` | double | Maximum distance table for data association | -| `max_area_matrix` | double | Maximum area table for data association | -| `min_area_matrix` | double | Minimum area table for data association | -| `max_rad_matrix` | double | Maximum angle table for data association | +{{ json_to_markdown("perception/multi_object_tracker/schema/multi_object_tracker_node.schema.json") }} +{{ json_to_markdown("perception/multi_object_tracker/schema/data_association_matrix.schema.json") }} + +#### Simulation parameters + +{{ json_to_markdown("perception/multi_object_tracker/schema/simulation_tracker.schema.json") }} ## Assumptions / Known limits diff --git a/perception/multi_object_tracker/config/data_association_matrix.param.yaml b/perception/autoware_multi_object_tracker/config/data_association_matrix.param.yaml similarity index 100% rename from perception/multi_object_tracker/config/data_association_matrix.param.yaml rename to perception/autoware_multi_object_tracker/config/data_association_matrix.param.yaml diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml similarity index 100% rename from perception/multi_object_tracker/config/input_channels.param.yaml rename to perception/autoware_multi_object_tracker/config/input_channels.param.yaml diff --git a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml similarity index 100% rename from perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml rename to perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml diff --git a/perception/multi_object_tracker/config/simulation_tracker.param.yaml b/perception/autoware_multi_object_tracker/config/simulation_tracker.param.yaml similarity index 100% rename from perception/multi_object_tracker/config/simulation_tracker.param.yaml rename to perception/autoware_multi_object_tracker/config/simulation_tracker.param.yaml diff --git a/perception/multi_object_tracker/image/anchor_point.drawio.svg b/perception/autoware_multi_object_tracker/image/anchor_point.drawio.svg similarity index 100% rename from perception/multi_object_tracker/image/anchor_point.drawio.svg rename to perception/autoware_multi_object_tracker/image/anchor_point.drawio.svg diff --git a/perception/multi_object_tracker/image/kinematic_bicycle_model.png b/perception/autoware_multi_object_tracker/image/kinematic_bicycle_model.png similarity index 100% rename from perception/multi_object_tracker/image/kinematic_bicycle_model.png rename to perception/autoware_multi_object_tracker/image/kinematic_bicycle_model.png diff --git a/perception/multi_object_tracker/image/multi_object_tracker_overview.svg b/perception/autoware_multi_object_tracker/image/multi_object_tracker_overview.svg similarity index 100% rename from perception/multi_object_tracker/image/multi_object_tracker_overview.svg rename to perception/autoware_multi_object_tracker/image/multi_object_tracker_overview.svg diff --git a/perception/multi_object_tracker/image/mussp_evaluation1.png b/perception/autoware_multi_object_tracker/image/mussp_evaluation1.png similarity index 100% rename from perception/multi_object_tracker/image/mussp_evaluation1.png rename to perception/autoware_multi_object_tracker/image/mussp_evaluation1.png diff --git a/perception/multi_object_tracker/image/mussp_evaluation2.png b/perception/autoware_multi_object_tracker/image/mussp_evaluation2.png similarity index 100% rename from perception/multi_object_tracker/image/mussp_evaluation2.png rename to perception/autoware_multi_object_tracker/image/mussp_evaluation2.png diff --git a/perception/multi_object_tracker/image/nearest_corner_or_side.drawio.svg b/perception/autoware_multi_object_tracker/image/nearest_corner_or_side.drawio.svg similarity index 100% rename from perception/multi_object_tracker/image/nearest_corner_or_side.drawio.svg rename to perception/autoware_multi_object_tracker/image/nearest_corner_or_side.drawio.svg diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/solver/gnn_solver_interface.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/solver/mu_ssp.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/solver/ssp.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp diff --git a/perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp similarity index 100% rename from perception/multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml similarity index 52% rename from perception/multi_object_tracker/launch/multi_object_tracker.launch.xml rename to perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml index b00ccd8fa623e..db76e181a6afa 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/autoware_multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -2,11 +2,11 @@ - - - + + + - + diff --git a/perception/multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp similarity index 100% rename from perception/multi_object_tracker/lib/association/association.cpp rename to perception/autoware_multi_object_tracker/lib/association/association.cpp diff --git a/perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp b/perception/autoware_multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp similarity index 100% rename from perception/multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp rename to perception/autoware_multi_object_tracker/lib/association/mu_successive_shortest_path/mu_ssp.cpp diff --git a/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp b/perception/autoware_multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp similarity index 98% rename from perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp rename to perception/autoware_multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp index d20992c7d183b..84333fe8a1ff6 100644 --- a/perception/multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/successive_shortest_path/ssp.cpp @@ -330,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); } } diff --git a/perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp diff --git a/perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp diff --git a/perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp diff --git a/perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp diff --git a/perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp diff --git a/perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp diff --git a/perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp diff --git a/perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/tracker_base.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp diff --git a/perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/model/unknown_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp diff --git a/perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp diff --git a/perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp diff --git a/perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp diff --git a/perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp similarity index 100% rename from perception/multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/motion_model/motion_model_base.cpp diff --git a/perception/multi_object_tracker/models.md b/perception/autoware_multi_object_tracker/models.md similarity index 100% rename from perception/multi_object_tracker/models.md rename to perception/autoware_multi_object_tracker/models.md diff --git a/perception/multi_object_tracker/package.xml b/perception/autoware_multi_object_tracker/package.xml similarity index 90% rename from perception/multi_object_tracker/package.xml rename to perception/autoware_multi_object_tracker/package.xml index 67f56273e79c5..3d941239976ea 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/autoware_multi_object_tracker/package.xml @@ -1,9 +1,9 @@ - multi_object_tracker + autoware_multi_object_tracker 1.0.0 - The ROS 2 multi_object_tracker package + The ROS 2 autoware_multi_object_tracker package Yukihiro Saito Yoshi Ri Taekjin Lee diff --git a/perception/autoware_multi_object_tracker/schema/data_association_matrix.schema.json b/perception/autoware_multi_object_tracker/schema/data_association_matrix.schema.json new file mode 100644 index 0000000000000..daf9d9f957b7c --- /dev/null +++ b/perception/autoware_multi_object_tracker/schema/data_association_matrix.schema.json @@ -0,0 +1,74 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Data Association Matrix", + "type": "object", + "definitions": { + "data_association_matrix": { + "type": "object", + "properties": { + "can_assign_matrix": { + "type": "array", + "description": "Assignment table for data association.", + "items": { + "type": "integer" + } + }, + "max_dist_matrix": { + "type": "array", + "description": "Maximum distance table for data association.", + "items": { + "type": "number" + } + }, + "max_area_matrix": { + "type": "array", + "description": "Maximum area table for data association.", + "items": { + "type": "number" + } + }, + "min_area_matrix": { + "type": "array", + "description": "Minimum area table for data association.", + "items": { + "type": "number" + } + }, + "max_rad_matrix": { + "type": "array", + "description": "Maximum angle table for data association.", + "items": { + "type": "number" + } + }, + "min_iou_matrix": { + "type": "array", + "description": "A matrix that represents the minimum Intersection over Union (IoU) limit allowed for assignment.", + "items": { + "type": "number" + } + } + }, + "required": [ + "can_assign_matrix", + "max_dist_matrix", + "max_area_matrix", + "min_area_matrix", + "max_rad_matrix", + "min_iou_matrix" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/data_association_matrix" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json new file mode 100644 index 0000000000000..3c3da4d3f70a0 --- /dev/null +++ b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json @@ -0,0 +1,80 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Input Channels", + "type": "object", + "definitions": { + "input_channel": { + "type": "object", + "properties": { + "topic": { + "type": "string", + "description": "The ROS topic name for the input channel." + }, + "can_spawn_new_tracker": { + "type": "boolean", + "description": "Indicates if the input channel can spawn new trackers." + }, + "optional": { + "type": "object", + "properties": { + "name": { + "type": "string", + "description": "The name of the input channel." + }, + "short_name": { + "type": "string", + "description": "The short name of the input channel." + } + } + } + }, + "required": ["topic", "can_spawn_new_tracker"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "input_channels": { + "type": "object", + "properties": { + "detected_objects": { "$ref": "#/definitions/input_channel" }, + "lidar_clustering": { "$ref": "#/definitions/input_channel" }, + "lidar_centerpoint": { "$ref": "#/definitions/input_channel" }, + "lidar_centerpoint_validated": { "$ref": "#/definitions/input_channel" }, + "lidar_apollo": { "$ref": "#/definitions/input_channel" }, + "lidar_apollo_validated": { "$ref": "#/definitions/input_channel" }, + "lidar_pointpainitng": { "$ref": "#/definitions/input_channel" }, + "lidar_pointpainting_validated": { "$ref": "#/definitions/input_channel" }, + "camera_lidar_fusion": { "$ref": "#/definitions/input_channel" }, + "detection_by_tracker": { "$ref": "#/definitions/input_channel" }, + "radar": { "$ref": "#/definitions/input_channel" }, + "radar_far": { "$ref": "#/definitions/input_channel" } + }, + "required": [ + "detected_objects", + "lidar_clustering", + "lidar_centerpoint", + "lidar_centerpoint_validated", + "lidar_apollo", + "lidar_apollo_validated", + "lidar_pointpainitng", + "lidar_pointpainting_validated", + "camera_lidar_fusion", + "detection_by_tracker", + "radar", + "radar_far" + ] + } + }, + "required": ["input_channels"] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json b/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json new file mode 100644 index 0000000000000..a40eb12d99b38 --- /dev/null +++ b/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json @@ -0,0 +1,116 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Multi Object Tracker Node", + "type": "object", + "definitions": { + "multi_object_tracker_node": { + "type": "object", + "properties": { + "car_tracker": { + "type": "string", + "description": "Tracker model for car class.", + "default": "multi_vehicle_tracker" + }, + "truck_tracker": { + "type": "string", + "description": "Tracker model for truck class.", + "default": "multi_vehicle_tracker" + }, + "bus_tracker": { + "type": "string", + "description": "Tracker model for bus class.", + "default": "multi_vehicle_tracker" + }, + "trailer_tracker": { + "type": "string", + "description": "Tracker model for trailer class.", + "default": "multi_vehicle_tracker" + }, + "pedestrian_tracker": { + "type": "string", + "description": "Tracker model for pedestrian class.", + "default": "pedestrian_and_bicycle_tracker" + }, + "bicycle_tracker": { + "type": "string", + "description": "Tracker model for bicycle class.", + "default": "pedestrian_and_bicycle_tracker" + }, + "motorcycle_tracker": { + "type": "string", + "description": "Tracker model for motorcycle class.", + "default": "pedestrian_and_bicycle_tracker" + }, + "publish_rate": { + "type": "number", + "description": "Timer frequency to output with delay compensation.", + "default": 10.0 + }, + "world_frame_id": { + "type": "string", + "description": "Object kinematics definition frame.", + "default": "map" + }, + "enable_delay_compensation": { + "type": "boolean", + "description": "If True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp.", + "default": false + }, + "publish_processing_time": { + "type": "boolean", + "description": "Enable to publish debug message of process time information.", + "default": false + }, + "publish_tentative_objects": { + "type": "boolean", + "description": "Enable to publish tentative tracked objects, which have lower confidence.", + "default": false + }, + "publish_debug_markers": { + "type": "boolean", + "description": "Enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection.", + "default": false + }, + "diagnostics_warn_delay": { + "type": "number", + "description": "Delay threshold for warning diagnostics in seconds.", + "default": 0.5 + }, + "diagnostics_error_delay": { + "type": "number", + "description": "Delay threshold for error diagnostics in seconds.", + "default": 1.0 + } + }, + "required": [ + "car_tracker", + "truck_tracker", + "bus_tracker", + "trailer_tracker", + "pedestrian_tracker", + "bicycle_tracker", + "motorcycle_tracker", + "publish_rate", + "world_frame_id", + "enable_delay_compensation", + "publish_processing_time", + "publish_tentative_objects", + "publish_debug_markers", + "diagnostics_warn_delay", + "diagnostics_error_delay" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/multi_object_tracker_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/autoware_multi_object_tracker/schema/simulation_tracker.schema.json b/perception/autoware_multi_object_tracker/schema/simulation_tracker.schema.json new file mode 100644 index 0000000000000..88a4cf41bad08 --- /dev/null +++ b/perception/autoware_multi_object_tracker/schema/simulation_tracker.schema.json @@ -0,0 +1,62 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Simulation Tracker Node", + "type": "object", + "definitions": { + "simulation_tracker_node": { + "type": "object", + "properties": { + "car_tracker": { + "type": "string", + "description": "Tracker model for car class.", + "default": "pass_through_tracker" + }, + "truck_tracker": { + "type": "string", + "description": "Tracker model for truck class.", + "default": "pass_through_tracker" + }, + "bus_tracker": { + "type": "string", + "description": "Tracker model for bus class.", + "default": "pass_through_tracker" + }, + "pedestrian_tracker": { + "type": "string", + "description": "Tracker model for pedestrian class.", + "default": "pass_through_tracker" + }, + "bicycle_tracker": { + "type": "string", + "description": "Tracker model for bicycle class.", + "default": "pass_through_tracker" + }, + "motorcycle_tracker": { + "type": "string", + "description": "Tracker model for motorcycle class.", + "default": "pass_through_tracker" + } + }, + "required": [ + "car_tracker", + "truck_tracker", + "bus_tracker", + "pedestrian_tracker", + "bicycle_tracker", + "motorcycle_tracker" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/simulation_tracker_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp similarity index 100% rename from perception/multi_object_tracker/src/debugger/debug_object.cpp rename to perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp diff --git a/perception/multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp similarity index 100% rename from perception/multi_object_tracker/src/debugger/debug_object.hpp rename to perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp similarity index 100% rename from perception/multi_object_tracker/src/debugger/debugger.cpp rename to perception/autoware_multi_object_tracker/src/debugger/debugger.cpp diff --git a/perception/multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp similarity index 100% rename from perception/multi_object_tracker/src/debugger/debugger.hpp rename to perception/autoware_multi_object_tracker/src/debugger/debugger.hpp diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp similarity index 100% rename from perception/multi_object_tracker/src/multi_object_tracker_node.cpp rename to perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp similarity index 100% rename from perception/multi_object_tracker/src/multi_object_tracker_node.hpp rename to perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp similarity index 100% rename from perception/multi_object_tracker/src/processor/input_manager.cpp rename to perception/autoware_multi_object_tracker/src/processor/input_manager.cpp diff --git a/perception/multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp similarity index 100% rename from perception/multi_object_tracker/src/processor/input_manager.hpp rename to perception/autoware_multi_object_tracker/src/processor/input_manager.hpp diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp similarity index 100% rename from perception/multi_object_tracker/src/processor/processor.cpp rename to perception/autoware_multi_object_tracker/src/processor/processor.cpp diff --git a/perception/multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp similarity index 97% rename from perception/multi_object_tracker/src/processor/processor.hpp rename to perception/autoware_multi_object_tracker/src/processor/processor.hpp index 5eac113b3974c..851a0f6a26717 100644 --- a/perception/multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -41,7 +41,7 @@ class TrackerProcessor // tracker processes void predict(const rclcpp::Time & time); void update( - const autoware_perception_msgs::msg::DetectedObjects & transformed_objects, + const autoware_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment, const uint & channel_index); void spawn( diff --git a/perception/object_merger/CMakeLists.txt b/perception/autoware_object_merger/CMakeLists.txt similarity index 96% rename from perception/object_merger/CMakeLists.txt rename to perception/autoware_object_merger/CMakeLists.txt index 2d9b490945caf..3a2e85c4060ea 100644 --- a/perception/object_merger/CMakeLists.txt +++ b/perception/autoware_object_merger/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(object_merger) +project(autoware_object_merger) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/object_merger/README.md b/perception/autoware_object_merger/README.md similarity index 100% rename from perception/object_merger/README.md rename to perception/autoware_object_merger/README.md diff --git a/perception/object_merger/config/data_association_matrix.param.yaml b/perception/autoware_object_merger/config/data_association_matrix.param.yaml similarity index 100% rename from perception/object_merger/config/data_association_matrix.param.yaml rename to perception/autoware_object_merger/config/data_association_matrix.param.yaml diff --git a/perception/object_merger/config/object_association_merger.param.yaml b/perception/autoware_object_merger/config/object_association_merger.param.yaml similarity index 100% rename from perception/object_merger/config/object_association_merger.param.yaml rename to perception/autoware_object_merger/config/object_association_merger.param.yaml diff --git a/perception/object_merger/config/overlapped_judge.param.yaml b/perception/autoware_object_merger/config/overlapped_judge.param.yaml similarity index 100% rename from perception/object_merger/config/overlapped_judge.param.yaml rename to perception/autoware_object_merger/config/overlapped_judge.param.yaml diff --git a/perception/object_merger/include/autoware_object_merger/association/data_association.hpp b/perception/autoware_object_merger/include/autoware/object_merger/association/data_association.hpp similarity index 86% rename from perception/object_merger/include/autoware_object_merger/association/data_association.hpp rename to perception/autoware_object_merger/include/autoware/object_merger/association/data_association.hpp index 8433e8f7af537..16e154c83842b 100644 --- a/perception/object_merger/include/autoware_object_merger/association/data_association.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/association/data_association.hpp @@ -16,12 +16,12 @@ // Author: v1.0 Yukihiro Saito // -#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef AUTOWARE__OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define AUTOWARE__OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "autoware_object_merger/association/solver/gnn_solver.hpp" +#include "autoware/object_merger/association/solver/gnn_solver.hpp" #include #include @@ -62,4 +62,4 @@ class DataAssociation } // namespace autoware::object_merger -#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#endif // AUTOWARE__OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver.hpp b/perception/autoware_object_merger/include/autoware/object_merger/association/solver/gnn_solver.hpp similarity index 60% rename from perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver.hpp rename to perception/autoware_object_merger/include/autoware/object_merger/association/solver/gnn_solver.hpp index 6421dd6108a4a..14078451612ec 100644 --- a/perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp" -#include "autoware_object_merger/association/solver/mu_ssp.hpp" -#include "autoware_object_merger/association/solver/ssp.hpp" +#include "autoware/object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware/object_merger/association/solver/mu_ssp.hpp" +#include "autoware/object_merger/association/solver/ssp.hpp" -#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver_interface.hpp b/perception/autoware_object_merger/include/autoware/object_merger/association/solver/gnn_solver_interface.hpp similarity index 81% rename from perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver_interface.hpp rename to perception/autoware_object_merger/include/autoware/object_merger/association/solver/gnn_solver_interface.hpp index 6a0702f056ba7..75f45b6eb5f3b 100644 --- a/perception/object_merger/include/autoware_object_merger/association/solver/gnn_solver_interface.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/association/solver/gnn_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include @@ -32,4 +32,4 @@ class GnnSolverInterface }; } // namespace autoware::object_merger::gnn_solver -#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/object_merger/include/autoware_object_merger/association/solver/mu_ssp.hpp b/perception/autoware_object_merger/include/autoware/object_merger/association/solver/mu_ssp.hpp similarity index 79% rename from perception/object_merger/include/autoware_object_merger/association/solver/mu_ssp.hpp rename to perception/autoware_object_merger/include/autoware/object_merger/association/solver/mu_ssp.hpp index 364e9306112a7..4d8075874cbca 100644 --- a/perception/object_merger/include/autoware_object_merger/association/solver/mu_ssp.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/association/solver/mu_ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#ifndef AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware/object_merger/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface }; } // namespace autoware::object_merger::gnn_solver -#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#endif // AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/object_merger/include/autoware_object_merger/association/solver/ssp.hpp b/perception/autoware_object_merger/include/autoware/object_merger/association/solver/ssp.hpp similarity index 80% rename from perception/object_merger/include/autoware_object_merger/association/solver/ssp.hpp rename to perception/autoware_object_merger/include/autoware/object_merger/association/solver/ssp.hpp index 41bf390829ade..94563981de011 100644 --- a/perception/object_merger/include/autoware_object_merger/association/solver/ssp.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/association/solver/ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ -#define AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#ifndef AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "autoware_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware/object_merger/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface }; } // namespace autoware::object_merger::gnn_solver -#endif // AUTOWARE_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#endif // AUTOWARE__OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/object_merger/src/object_association_merger_node.hpp b/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp similarity index 90% rename from perception/object_merger/src/object_association_merger_node.hpp rename to perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp index d5b5a1ae018a6..81fa34803d6cc 100644 --- a/perception/object_merger/src/object_association_merger_node.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER_NODE_HPP_ -#define OBJECT_ASSOCIATION_MERGER_NODE_HPP_ +#ifndef AUTOWARE__OBJECT_MERGER__OBJECT_ASSOCIATION_MERGER_NODE_HPP_ +#define AUTOWARE__OBJECT_MERGER__OBJECT_ASSOCIATION_MERGER_NODE_HPP_ +#include "autoware/object_merger/association/data_association.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "autoware_object_merger/association/data_association.hpp" #include @@ -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_; @@ -91,4 +91,4 @@ class ObjectAssociationMergerNode : public rclcpp::Node }; } // namespace autoware::object_merger -#endif // OBJECT_ASSOCIATION_MERGER_NODE_HPP_ +#endif // AUTOWARE__OBJECT_MERGER__OBJECT_ASSOCIATION_MERGER_NODE_HPP_ diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/autoware_object_merger/launch/object_association_merger.launch.xml similarity index 74% rename from perception/object_merger/launch/object_association_merger.launch.xml rename to perception/autoware_object_merger/launch/object_association_merger.launch.xml index b26788bb04667..f3c0e8bd5a829 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/autoware_object_merger/launch/object_association_merger.launch.xml @@ -4,11 +4,11 @@ - - - + + + - + diff --git a/perception/object_merger/package.xml b/perception/autoware_object_merger/package.xml similarity index 91% rename from perception/object_merger/package.xml rename to perception/autoware_object_merger/package.xml index 5ca261b3ddf0e..e87ef8d81b01b 100644 --- a/perception/object_merger/package.xml +++ b/perception/autoware_object_merger/package.xml @@ -1,9 +1,9 @@ - object_merger + autoware_object_merger 0.1.0 - The object_merger package + The autoware_object_merger package Yukihiro Saito Yoshi Ri Taekjin Lee diff --git a/perception/object_merger/schema/data_association_matrix.schema.json b/perception/autoware_object_merger/schema/data_association_matrix.schema.json similarity index 100% rename from perception/object_merger/schema/data_association_matrix.schema.json rename to perception/autoware_object_merger/schema/data_association_matrix.schema.json diff --git a/perception/object_merger/schema/object_association_merger.schema.json b/perception/autoware_object_merger/schema/object_association_merger.schema.json similarity index 100% rename from perception/object_merger/schema/object_association_merger.schema.json rename to perception/autoware_object_merger/schema/object_association_merger.schema.json diff --git a/perception/object_merger/schema/overlapped_judge.schema.json b/perception/autoware_object_merger/schema/overlapped_judge.schema.json similarity index 100% rename from perception/object_merger/schema/overlapped_judge.schema.json rename to perception/autoware_object_merger/schema/overlapped_judge.schema.json diff --git a/perception/object_merger/src/association/data_association.cpp b/perception/autoware_object_merger/src/association/data_association.cpp similarity index 98% rename from perception/object_merger/src/association/data_association.cpp rename to perception/autoware_object_merger/src/association/data_association.cpp index e317ac63d9831..8b40178b592f8 100644 --- a/perception/object_merger/src/association/data_association.cpp +++ b/perception/autoware_object_merger/src/association/data_association.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_object_merger/association/data_association.hpp" +#include "autoware/object_merger/association/data_association.hpp" +#include "autoware/object_merger/association/solver/gnn_solver.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware_object_merger/association/solver/gnn_solver.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include diff --git a/perception/object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp b/perception/autoware_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp similarity index 95% rename from perception/object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp rename to perception/autoware_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp index 6a5dd6473da1f..f852cfb62d419 100644 --- a/perception/object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp +++ b/perception/autoware_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_object_merger/association/solver/mu_ssp.hpp" +#include "autoware/object_merger/association/solver/mu_ssp.hpp" #include diff --git a/perception/object_merger/src/association/solver/successive_shortest_path.cpp b/perception/autoware_object_merger/src/association/solver/successive_shortest_path.cpp similarity index 99% rename from perception/object_merger/src/association/solver/successive_shortest_path.cpp rename to perception/autoware_object_merger/src/association/solver/successive_shortest_path.cpp index e384f12d60051..b97db07d05469 100644 --- a/perception/object_merger/src/association/solver/successive_shortest_path.cpp +++ b/perception/autoware_object_merger/src/association/solver/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_object_merger/association/solver/ssp.hpp" +#include "autoware/object_merger/association/solver/ssp.hpp" #include #include diff --git a/perception/object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp similarity index 99% rename from perception/object_merger/src/object_association_merger_node.cpp rename to perception/autoware_object_merger/src/object_association_merger_node.cpp index 321f1a8da8d9e..a1021df1d555f 100644 --- a/perception/object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -14,7 +14,7 @@ #define EIGEN_MPL2_ONLY -#include "object_association_merger_node.hpp" +#include "autoware/object_merger/object_association_merger_node.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" diff --git a/perception/autoware_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 index 545b7d7e41b2a..b82b915898f53 100644 --- a/perception/autoware_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 @@ -99,9 +99,9 @@ class RadarFusionToDetectedObject // std::vector splitObject( // const DetectedObject & object, const std::shared_ptr> & radars); TwistWithCovariance estimateTwist( - const DetectedObject & object, std::shared_ptr> & radars); + const DetectedObject & object, const std::shared_ptr> & radars); bool isQualified( - const DetectedObject & object, std::shared_ptr> & radars); + const DetectedObject & object, const std::shared_ptr> & radars); TwistWithCovariance convertDopplerToTwist( const DetectedObject & object, const TwistWithCovariance & twist_with_covariance); bool isYawCorrect( diff --git a/perception/autoware_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 index 0fb87c0f88ba0..bc96ac780fd0a 100644 --- a/perception/autoware_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 @@ -205,7 +205,7 @@ RadarFusionToDetectedObject::filterRadarWithinObject( // (Target value is amplitude if using radar pointcloud. Target value is probability if using radar // objects). TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( - const DetectedObject & object, std::shared_ptr> & radars) + const DetectedObject & object, const std::shared_ptr> & radars) { if (!radars || (*radars).empty()) { TwistWithCovariance output{}; @@ -298,7 +298,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( // Judge whether low confidence objects that do not have some radar points/objects or not. bool RadarFusionToDetectedObject::isQualified( - const DetectedObject & object, std::shared_ptr> & radars) + const DetectedObject & object, const std::shared_ptr> & radars) { if (object.existence_probability > param_.threshold_probability) { return true; diff --git a/perception/autoware_radar_object_clustering/README.md b/perception/autoware_radar_object_clustering/README.md index 5fbb4df81a115..bb792185299ab 100644 --- a/perception/autoware_radar_object_clustering/README.md +++ b/perception/autoware_radar_object_clustering/README.md @@ -44,7 +44,7 @@ When the size information from radar outputs lack accuracy, `is_fixed_size` para If the parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to `size_x`, `size_y`, `size_z`, as average of vehicle size. -Note that to use for [multi_objects_tracker](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/multi_object_tracker), the size parameters need to exceed `min_area_matrix` parameters of it. +Note that to use for [multi_objects_tracker](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_multi_object_tracker), the size parameters need to exceed `min_area_matrix` parameters of it. ### Limitation diff --git a/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.hpp b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.hpp index aaba15bbad05d..224fa10e45c32 100644 --- a/perception/autoware_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/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 100% 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 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 100% 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 diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/autoware_tracking_object_merger/CMakeLists.txt similarity index 95% rename from perception/tracking_object_merger/CMakeLists.txt rename to perception/autoware_tracking_object_merger/CMakeLists.txt index 7e7c698365922..9b17ee0546879 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/autoware_tracking_object_merger/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(tracking_object_merger VERSION 1.0.0) +project(autoware_tracking_object_merger VERSION 1.0.0) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wconversion) diff --git a/perception/tracking_object_merger/README.md b/perception/autoware_tracking_object_merger/README.md similarity index 100% rename from perception/tracking_object_merger/README.md rename to perception/autoware_tracking_object_merger/README.md diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/autoware_tracking_object_merger/config/data_association_matrix.param.yaml similarity index 100% rename from perception/tracking_object_merger/config/data_association_matrix.param.yaml rename to perception/autoware_tracking_object_merger/config/data_association_matrix.param.yaml diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/autoware_tracking_object_merger/config/decorative_tracker_merger.param.yaml similarity index 100% rename from perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml rename to perception/autoware_tracking_object_merger/config/decorative_tracker_merger.param.yaml diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml b/perception/autoware_tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml similarity index 100% rename from perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml rename to perception/autoware_tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml diff --git a/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg b/perception/autoware_tracking_object_merger/image/decorative_tracker_merger.drawio.svg similarity index 100% rename from perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg rename to perception/autoware_tracking_object_merger/image/decorative_tracker_merger.drawio.svg diff --git a/perception/tracking_object_merger/image/time_sync.drawio.svg b/perception/autoware_tracking_object_merger/image/time_sync.drawio.svg similarity index 100% rename from perception/tracking_object_merger/image/time_sync.drawio.svg rename to perception/autoware_tracking_object_merger/image/time_sync.drawio.svg diff --git a/perception/tracking_object_merger/image/tracklet_management.drawio.svg b/perception/autoware_tracking_object_merger/image/tracklet_management.drawio.svg similarity index 100% rename from perception/tracking_object_merger/image/tracklet_management.drawio.svg rename to perception/autoware_tracking_object_merger/image/tracklet_management.drawio.svg diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp similarity index 86% rename from perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp rename to perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp index a272e42a74e80..af4a65869eca5 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp @@ -16,13 +16,13 @@ // Author: v1.0 Yukihiro Saito // -#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "autoware_tracking_object_merger/association/solver/gnn_solver.hpp" -#include "autoware_tracking_object_merger/utils/tracker_state.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" +#include "autoware/tracking_object_merger/utils/tracker_state.hpp" #include #include @@ -72,4 +72,4 @@ class DataAssociation } // namespace autoware::tracking_object_merger -#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp similarity index 59% rename from perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp rename to perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp index 4cea0e3cbe96f..64a18d878f7c3 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp" -#include "autoware_tracking_object_merger/association/solver/mu_ssp.hpp" -#include "autoware_tracking_object_merger/association/solver/ssp.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware/tracking_object_merger/association/solver/mu_ssp.hpp" +#include "autoware/tracking_object_merger/association/solver/ssp.hpp" -#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp similarity index 79% rename from perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp rename to perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp index d751075773a09..732ed5cd87041 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include @@ -32,4 +32,4 @@ class GnnSolverInterface }; } // namespace autoware::tracking_object_merger::gnn_solver -#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp similarity index 78% rename from perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp rename to perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp index 47c23bc8eb1b1..b219c1ac441eb 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface }; } // namespace autoware::tracking_object_merger::gnn_solver -#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp similarity index 79% rename from perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp rename to perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp index bcfac73f3e43b..03311e4e43694 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ -#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface }; } // namespace autoware::tracking_object_merger::gnn_solver -#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp similarity index 88% rename from perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp rename to perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp index ae19f51876490..f091959396574 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DECORATIVE_TRACKER_MERGER_NODE_HPP_ -#define DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#include "autoware/tracking_object_merger/association/data_association.hpp" +#include "autoware/tracking_object_merger/utils/tracker_state.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "autoware_tracking_object_merger/association/data_association.hpp" -#include "autoware_tracking_object_merger/utils/tracker_state.hpp" -#include "autoware_tracking_object_merger/utils/utils.hpp" #include @@ -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, @@ -133,4 +133,4 @@ class DecorativeTrackerMergerNode : public rclcpp::Node } // namespace autoware::tracking_object_merger -#endif // DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp similarity index 96% rename from perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp rename to perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp index ed1f69191b652..5f1ab36f1265e 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ -#define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ #include #include @@ -149,4 +149,4 @@ TrackedObjects getTrackedObjectsFromTrackerStates( std::vector & tracker_states, const rclcpp::Time & time); } // namespace autoware::tracking_object_merger -#endif // AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp similarity index 94% rename from perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp rename to perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp index eb288c4e853ad..17a385da7cb41 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp @@ -14,8 +14,8 @@ // // -#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ -#define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ #include "autoware/universe_utils/geometry/geometry.hpp" @@ -100,4 +100,4 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su } // namespace autoware::tracking_object_merger -#endif // AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml similarity index 52% rename from perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml rename to perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml index 2cc2a69e295e6..cd609a0fa612a 100644 --- a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml +++ b/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -3,11 +3,11 @@ - - - + + + - + diff --git a/perception/tracking_object_merger/package.xml b/perception/autoware_tracking_object_merger/package.xml similarity index 96% rename from perception/tracking_object_merger/package.xml rename to perception/autoware_tracking_object_merger/package.xml index cc23578773fe2..321016ac0ff48 100644 --- a/perception/tracking_object_merger/package.xml +++ b/perception/autoware_tracking_object_merger/package.xml @@ -1,7 +1,7 @@ - tracking_object_merger + autoware_tracking_object_merger 0.0.0 merge tracking object Yukihiro Saito diff --git a/perception/tracking_object_merger/src/association/data_association.cpp b/perception/autoware_tracking_object_merger/src/association/data_association.cpp similarity index 97% rename from perception/tracking_object_merger/src/association/data_association.cpp rename to perception/autoware_tracking_object_merger/src/association/data_association.cpp index 046fcf5986469..119c95c86daf8 100644 --- a/perception/tracking_object_merger/src/association/data_association.cpp +++ b/perception/autoware_tracking_object_merger/src/association/data_association.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_tracking_object_merger/association/data_association.hpp" +#include "autoware/tracking_object_merger/association/data_association.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware_tracking_object_merger/association/solver/gnn_solver.hpp" -#include "autoware_tracking_object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include diff --git a/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp b/perception/autoware_tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp similarity index 95% rename from perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp rename to perception/autoware_tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp index d2db6eba068ec..7899d7e1a7898 100644 --- a/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp +++ b/perception/autoware_tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_tracking_object_merger/association/solver/mu_ssp.hpp" +#include "autoware/tracking_object_merger/association/solver/mu_ssp.hpp" #include diff --git a/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp b/perception/autoware_tracking_object_merger/src/association/solver/successive_shortest_path.cpp similarity index 99% rename from perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp rename to perception/autoware_tracking_object_merger/src/association/solver/successive_shortest_path.cpp index 7d792c157d356..e56901e479eac 100644 --- a/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp +++ b/perception/autoware_tracking_object_merger/src/association/solver/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_tracking_object_merger/association/solver/ssp.hpp" +#include "autoware/tracking_object_merger/association/solver/ssp.hpp" #include #include diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp similarity index 98% rename from perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp rename to perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp index 6f65941c0a747..187fb6e1d462e 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -14,10 +14,10 @@ #define EIGEN_MPL2_ONLY -#include "decorative_tracker_merger_node.hpp" +#include "autoware/tracking_object_merger/decorative_tracker_merger_node.hpp" -#include "autoware_tracking_object_merger/association/solver/ssp.hpp" -#include "autoware_tracking_object_merger/utils/utils.hpp" +#include "autoware/tracking_object_merger/association/solver/ssp.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp similarity index 98% rename from perception/tracking_object_merger/src/utils/tracker_state.cpp rename to perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp index 54b1d73c375a5..eabbd72364129 100644 --- a/perception/tracking_object_merger/src/utils/tracker_state.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_tracking_object_merger/utils/tracker_state.hpp" +#include "autoware/tracking_object_merger/utils/tracker_state.hpp" -#include "autoware_tracking_object_merger/utils/utils.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" namespace autoware::tracking_object_merger { diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/autoware_tracking_object_merger/src/utils/utils.cpp similarity index 99% rename from perception/tracking_object_merger/src/utils/utils.cpp rename to perception/autoware_tracking_object_merger/src/utils/utils.cpp index 1ae705aa38f15..3a24cc0f8321d 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_tracking_object_merger/utils/utils.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" #include "autoware_perception_msgs/msg/shape.hpp" #include "autoware_perception_msgs/msg/tracked_object.hpp" diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index bc54519a95e47..b13f2e537ee55 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -196,7 +196,7 @@ 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; diff --git a/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 3fe527935c485..c5ecce714735b 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -38,7 +38,7 @@ 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; diff --git a/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index 19b42ecf9055b..801a693905f93 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -64,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_; @@ -107,7 +107,7 @@ class Validator2D : public Validator 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 { diff --git a/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp index ce900f30f4255..c082b4b0f03f1 100644 --- a/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp @@ -163,11 +163,11 @@ void OccupancyGridBasedValidator::showDebugImage( auto mask = getMask(ros_occ_grid, object); const float mean = mask ? cv::mean(occ_grid, mask.value())[0] * 0.01 : 1.0; if (mean_threshold_ < mean) { - auto mask = getMask(ros_occ_grid, object, passed_objects_image); - if (mask) passed_objects_image = mask.value(); + auto passed_mask = getMask(ros_occ_grid, object, passed_objects_image); + if (passed_mask) passed_objects_image = passed_mask.value(); } else { - auto mask = getMask(ros_occ_grid, object, removed_objects_image); - if (mask) removed_objects_image = mask.value(); + auto removed_mask = getMask(ros_occ_grid, object, removed_objects_image); + if (removed_mask) removed_objects_image = removed_mask.value(); } } } 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 dd867c3cfba0e..01116c5bdce5f 100644 --- a/perception/ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter/node.cpp @@ -245,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; @@ -287,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; @@ -305,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; diff --git a/perception/ground_segmentation/src/scan_ground_filter/node.hpp b/perception/ground_segmentation/src/scan_ground_filter/node.hpp index 6de1d1b7ad09f..67a844eee75c5 100644 --- a/perception/ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/ground_segmentation/src/scan_ground_filter/node.hpp @@ -240,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 6be7defe422c3..48c285eeb3d88 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -32,22 +32,22 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "image_projection_based_fusion::RoiDetectedObjectFusionNode" + PLUGIN "autoware::image_projection_based_fusion::RoiDetectedObjectFusionNode" EXECUTABLE roi_detected_object_fusion_node ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "image_projection_based_fusion::RoiClusterFusionNode" + PLUGIN "autoware::image_projection_based_fusion::RoiClusterFusionNode" EXECUTABLE roi_cluster_fusion_node ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "image_projection_based_fusion::SegmentPointCloudFusionNode" + PLUGIN "autoware::image_projection_based_fusion::SegmentPointCloudFusionNode" EXECUTABLE segmentation_pointcloud_fusion_node ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" + PLUGIN "autoware::image_projection_based_fusion::RoiPointCloudFusionNode" EXECUTABLE roi_pointcloud_fusion_node ) @@ -149,7 +149,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) rclcpp_components_register_node(pointpainting_lib - PLUGIN "image_projection_based_fusion::PointPaintingFusionNode" + PLUGIN "autoware::image_projection_based_fusion::PointPaintingFusionNode" EXECUTABLE pointpainting_fusion_node ) 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/include/image_projection_based_fusion/debugger.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/debugger.hpp similarity index 86% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/debugger.hpp index e4b1913effed5..ba7f8b6ca3496 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/debugger.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__DEBUGGER_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__DEBUGGER_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__DEBUGGER_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__DEBUGGER_HPP_ #define EIGEN_MPL2_ONLY @@ -31,7 +31,7 @@ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { using sensor_msgs::msg::RegionOfInterest; @@ -66,6 +66,6 @@ class Debugger std::size_t image_buffer_size_; }; -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__DEBUGGER_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__DEBUGGER_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp similarity index 93% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index df8bf66433300..d4e498212ac36 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#include #include #include -#include #include #include @@ -44,7 +44,7 @@ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -142,6 +142,6 @@ class FusionNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp similarity index 80% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index 756cb4224bc20..964624614ec57 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ -#include "image_projection_based_fusion/fusion_node.hpp" -#include "image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" #include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" -#include -#include +#include +#include #include #include @@ -29,7 +29,7 @@ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -78,5 +78,5 @@ class PointPaintingFusionNode bool out_of_scope(const DetectedObjects & obj); }; -} // namespace image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ +} // namespace autoware::image_projection_based_fusion +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp similarity index 82% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp index 03609eb18e689..845db88467c15 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ #include #include @@ -29,7 +29,7 @@ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { struct PointCloudWithTransform { @@ -67,6 +67,7 @@ class PointCloudDensification std::list pointcloud_cache_; }; -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_ 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/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp similarity index 72% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index d28d9eb31216d..daf925c2ff9f3 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -12,18 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ -#include +#include #include #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { -static constexpr size_t CAPACITY_POINT = 1000000; class PointPaintingTRT : public centerpoint::CenterPointTRT { public: @@ -41,6 +40,6 @@ class PointPaintingTRT : public centerpoint::CenterPointTRT std::unique_ptr vg_ptr_pp_{nullptr}; }; -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp similarity index 81% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp index 897609fa3d86d..70a9fbb4a2244 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__PREPROCESS_KERNEL_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__PREPROCESS_KERNEL_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__PREPROCESS_KERNEL_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__PREPROCESS_KERNEL_HPP_ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { cudaError_t generateVoxels_random_launch( const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, @@ -38,6 +38,6 @@ cudaError_t generateFeatures_launch( const float range_min_y, const float range_min_z, float * features, const std::size_t encoder_in_feature_size, cudaStream_t stream); -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__PREPROCESS_KERNEL_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__PREPROCESS_KERNEL_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp similarity index 73% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp index 4cdca8e49ac7e..f303a98824ece 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ -#include +#include #include #include @@ -23,7 +23,7 @@ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { class VoxelGenerator @@ -45,6 +45,6 @@ class VoxelGenerator std::array grid_size_; std::array recip_voxel_size_; }; -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp similarity index 84% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index b9471f2f3b78e..abececee35c25 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_ -#include "image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" #include #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; @@ -61,6 +61,6 @@ class RoiClusterFusionNode // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_CLUSTER_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp similarity index 82% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index d61ced593de78..ea15a2df5efe2 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ +#include "autoware/image_projection_based_fusion/fusion_node.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" -#include "image_projection_based_fusion/fusion_node.hpp" -#include +#include #include "autoware_perception_msgs/msg/object_classification.hpp" @@ -26,7 +26,7 @@ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { using sensor_msgs::msg::RegionOfInterest; @@ -74,6 +74,6 @@ class RoiDetectedObjectFusionNode ignored_object_flags_map_; }; -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_DETECTED_OBJECT_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp similarity index 77% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 4d4fd8d2dac42..c7cad2b5a64d0 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ -#include "image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" -#include +#include #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { class RoiPointCloudFusionNode : public FusionNode @@ -52,5 +52,5 @@ class RoiPointCloudFusionNode bool out_of_scope(const DetectedObjectWithFeature & obj); }; -} // namespace image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +} // namespace autoware::image_projection_based_fusion +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ 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/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp similarity index 82% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 89b33775f7898..c57d455300125 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ -#include "image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" -#include +#include #include #include @@ -29,7 +29,7 @@ #include #endif -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { class SegmentPointCloudFusionNode : public FusionNode { @@ -66,5 +66,5 @@ class SegmentPointCloudFusionNode : public FusionNode -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { using autoware_perception_msgs::msg::Shape; @@ -66,6 +66,6 @@ bool is_inside( void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width, const int height); -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__GEOMETRY_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__UTILS__GEOMETRY_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp similarity index 89% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp rename to perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 107e0d6c179a8..702b085dd31d7 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ #define EIGEN_MPL2_ONLY @@ -32,7 +32,7 @@ #include #endif -#include "image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" #include #include @@ -51,7 +51,7 @@ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { using PointCloud = pcl::PointCloud; @@ -92,6 +92,6 @@ void addShapeAndKinematic( const pcl::PointCloud & cluster, tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion -#endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index d2f803f13d376..ed583c7a46b0a 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -45,7 +45,7 @@ - + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 00ffdc8fd5528..f7a95ae4c70d6 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -26,12 +26,9 @@ lidar_centerpoint message_filters object_recognition_utils - pcl_conversions - pcl_ros rclcpp rclcpp_components sensor_msgs - tf2 tf2_eigen tf2_ros tf2_sensor_msgs diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index a51c23a77aac6..90952f3f6a8f5 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/debugger.hpp" +#include "autoware/image_projection_based_fusion/debugger.hpp" #if __has_include() #include @@ -36,7 +36,7 @@ void drawRoiOnImage( } // namespace -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { Debugger::Debugger( rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size, @@ -154,4 +154,4 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta } } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 20273fd1de547..6cc4a0c23adcc 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -14,7 +14,7 @@ #define EIGEN_MPL2_ONLY -#include "image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" #include #include @@ -37,7 +37,7 @@ // static int publish_counter = 0; static double processing_time_ms = 0; -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { template @@ -437,4 +437,4 @@ template class FusionNode< template class FusionNode; template class FusionNode; template class FusionNode; -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion 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 ffeb47d3123a3..af6901f8988b3 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/pointpainting_fusion/node.hpp" +#include "autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp" #include "autoware_point_types/types.hpp" +#include +#include #include #include -#include -#include #include #include #include @@ -42,7 +42,7 @@ Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) } // namespace -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { inline bool isVehicle(int label2d) @@ -103,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"); @@ -181,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( @@ -322,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(); @@ -345,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 @@ -414,7 +418,7 @@ bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const Detecte { return false; } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion #include -RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::PointPaintingFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::image_projection_based_fusion::PointPaintingFusionNode) diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp index 9edf6a73cd59b..291baa392ecbc 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp" +#include "autoware/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp" #include @@ -54,7 +54,7 @@ Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) } // namespace -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { PointCloudDensification::PointCloudDensification(const centerpoint::DensificationParam & param) : param_(param) @@ -100,4 +100,4 @@ void PointCloudDensification::dequeue() } } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index c1e431ed83c99..59c747c8f1cfa 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" +#include "autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" +#include #include -#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { PointPaintingTRT::PointPaintingTRT( const centerpoint::NetworkParam & encoder_param, const centerpoint::NetworkParam & head_param, @@ -81,4 +81,4 @@ bool PointPaintingTRT::preprocess( return true; } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu index 68e08ac61a569..82ab7e954fad1 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu @@ -28,7 +28,7 @@ * limitations under the License. */ -#include "image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp" +#include "autoware/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp" #include // #include @@ -55,7 +55,7 @@ std::size_t divup(const std::size_t a, const std::size_t b) } // namespace -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { __global__ void generateVoxels_random_kernel( const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, @@ -269,4 +269,4 @@ cudaError_t generateFeatures_launch( return cudaGetLastError(); } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp old mode 100755 new mode 100644 index cb3fc33d3e022..b0c6d0f888b08 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp" +#include "autoware/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp" #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { VoxelGenerator::VoxelGenerator( @@ -83,4 +83,4 @@ size_t VoxelGenerator::generateSweepPoints(std::vector & points) return point_counter; } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 51987cbbcab84..d4d5de3cfd381 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/roi_cluster_fusion/node.hpp" +#include "autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp" -#include -#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #endif -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) @@ -290,7 +290,7 @@ double RoiClusterFusionNode::cal_iou_by_mode( } } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion #include -RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiClusterFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::image_projection_based_fusion::RoiClusterFusionNode) diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 7d85ecb2f9200..e2197c148bcf9 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/roi_detected_object_fusion/node.hpp" +#include "autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include -#include +#include +#include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) @@ -324,7 +324,8 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) ignored_object_flags_map_.erase(timestamp_nsec); } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion #include -RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiDetectedObjectFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::image_projection_based_fusion::RoiDetectedObjectFusionNode) 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 904e66cb53298..40406d6e420b7 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 @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/roi_pointcloud_fusion/node.hpp" +#include "autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp" -#include "image_projection_based_fusion/utils/geometry.hpp" -#include "image_projection_based_fusion/utils/utils.hpp" +#include "autoware/image_projection_based_fusion/utils/geometry.hpp" +#include "autoware/image_projection_based_fusion/utils/utils.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -27,7 +27,7 @@ #include "autoware/euclidean_cluster/utils.hpp" -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode( @@ -174,7 +174,7 @@ bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) { return false; } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion #include -RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiPointCloudFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::image_projection_based_fusion::RoiPointCloudFusionNode) 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 9a983252af436..5c93fa6c2fd1b 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 @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp" +#include "autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp" -#include "image_projection_based_fusion/utils/geometry.hpp" -#include "image_projection_based_fusion/utils/utils.hpp" +#include "autoware/image_projection_based_fusion/utils/geometry.hpp" +#include "autoware/image_projection_based_fusion/utils/utils.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -25,7 +25,7 @@ #include #endif -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode("segmentation_pointcloud_fusion", options) @@ -155,7 +155,8 @@ bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) { return false; } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion #include -RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::SegmentPointCloudFusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::image_projection_based_fusion::SegmentPointCloudFusionNode) diff --git a/perception/image_projection_based_fusion/src/utils/geometry.cpp b/perception/image_projection_based_fusion/src/utils/geometry.cpp index 29198280ec7b2..4bea6ac0fe713 100644 --- a/perception/image_projection_based_fusion/src/utils/geometry.cpp +++ b/perception/image_projection_based_fusion/src/utils/geometry.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/utils/geometry.hpp" +#include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { double calcIoU( @@ -204,4 +204,4 @@ void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width_, con } } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index aeec2886a801a..0432242417e19 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/utils/utils.hpp" +#include "autoware/image_projection_based_fusion/utils/utils.hpp" -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { Eigen::Vector2d calcRawImageProjectedPoint( const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d) @@ -277,4 +277,4 @@ pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) return closest_point; } -} // namespace image_projection_based_fusion +} // namespace autoware::image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp b/perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp index 1cddef44c0bac..8f2cf87d47bf2 100644 --- a/perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp +++ b/perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/utils/geometry.hpp" +#include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include -using image_projection_based_fusion::calcIoU; -using image_projection_based_fusion::calcIoUX; -using image_projection_based_fusion::calcIoUY; +using autoware::image_projection_based_fusion::calcIoU; +using autoware::image_projection_based_fusion::calcIoUX; +using autoware::image_projection_based_fusion::calcIoUY; TEST(GeometryTest, CalcIoU) { diff --git a/perception/image_projection_based_fusion/test/test_geometry.cpp b/perception/image_projection_based_fusion/test/test_geometry.cpp index eae5f7cef728c..cf3778e7a0c84 100644 --- a/perception/image_projection_based_fusion/test/test_geometry.cpp +++ b/perception/image_projection_based_fusion/test/test_geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_projection_based_fusion/utils/geometry.hpp" +#include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include @@ -37,7 +37,7 @@ TEST(objectToVertices, test_objectToVertices) shape.dimensions.z = 8.0; std::vector vertices; - image_projection_based_fusion::objectToVertices(pose, shape, vertices); + autoware::image_projection_based_fusion::objectToVertices(pose, shape, vertices); EXPECT_FALSE(vertices.empty()); EXPECT_NEAR(vertices.at(0).x(), 1.2320508075688772935274, 1e-6); @@ -57,7 +57,7 @@ TEST(objectToVertices, test_objectToVertices) shape.dimensions.z = 8.0; std::vector vertices; - image_projection_based_fusion::objectToVertices(pose, shape, vertices); + autoware::image_projection_based_fusion::objectToVertices(pose, shape, vertices); EXPECT_FALSE(vertices.empty()); EXPECT_NEAR(vertices.at(0).x(), 2.732050807568877293528, 1e-6); @@ -74,7 +74,7 @@ TEST(objectToVertices, test_objectToVertices) shape.type = 2; std::vector vertices; - image_projection_based_fusion::objectToVertices(pose, shape, vertices); + autoware::image_projection_based_fusion::objectToVertices(pose, shape, vertices); EXPECT_TRUE(vertices.empty()); } @@ -93,7 +93,8 @@ TEST(transformPoints, test_transformPoints) Eigen::Affine3d affine_transform = rotation * translation; std::vector output_points; - image_projection_based_fusion::transformPoints(input_points, affine_transform, output_points); + autoware::image_projection_based_fusion::transformPoints( + input_points, affine_transform, output_points); EXPECT_FALSE(output_points.empty()); EXPECT_NEAR(output_points.at(0).x(), 0.7071067811865475244008, 1e-6); @@ -118,7 +119,7 @@ TEST(is_inside, test_is_inside) inner.width = 299; const bool inside_flag = - image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + autoware::image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); EXPECT_TRUE(inside_flag); } @@ -130,7 +131,7 @@ TEST(is_inside, test_is_inside) inner.y_offset = 39; const bool inside_flag = - image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + autoware::image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); EXPECT_FALSE(inside_flag); } @@ -144,7 +145,7 @@ TEST(is_inside, test_is_inside) inner.width = 301; const bool inside_flag = - image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + autoware::image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); EXPECT_FALSE(inside_flag); } @@ -162,7 +163,7 @@ TEST(sanitizeROI, test_sanitizeROI) int height = 400; // image height int width = 300; // image width - image_projection_based_fusion::sanitizeROI(roi, width, height); + autoware::image_projection_based_fusion::sanitizeROI(roi, width, height); EXPECT_EQ(roi.height, 200); EXPECT_EQ(roi.width, 100); @@ -176,7 +177,7 @@ TEST(sanitizeROI, test_sanitizeROI) int height = 100; int width = 50; - image_projection_based_fusion::sanitizeROI(roi, width, height); + autoware::image_projection_based_fusion::sanitizeROI(roi, width, height); EXPECT_EQ(roi.height, 0); EXPECT_EQ(roi.width, 0); @@ -192,7 +193,7 @@ TEST(sanitizeROI, test_sanitizeROI) int height = 100; int width = 50; - image_projection_based_fusion::sanitizeROI(roi, width, height); + autoware::image_projection_based_fusion::sanitizeROI(roi, width, height); EXPECT_EQ(roi.height, 80); EXPECT_EQ(roi.width, 40); diff --git a/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp b/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp index 1c69273667997..6c7a692f48576 100644 --- a/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp +++ b/perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -30,27 +30,27 @@ sensor_msgs::msg::RegionOfInterest createRoi( 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); + bool result = autoware::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); + bool result = autoware::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); + bool result = autoware::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); + bool result = autoware::image_projection_based_fusion::isInsideBbox(0.0, 0.0, roi, 1.0); EXPECT_TRUE(result); } diff --git a/perception/image_projection_based_fusion/test/test_utils.cpp b/perception/image_projection_based_fusion/test/test_utils.cpp index e578ce1987cc3..5fbd313d75dfe 100644 --- a/perception/image_projection_based_fusion/test/test_utils.cpp +++ b/perception/image_projection_based_fusion/test/test_utils.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include @@ -65,7 +65,7 @@ TEST(UtilsTest, closest_cluster_empty_cluster_test_case1) double cluster_2d_tolerance = 0.5; int min_cluster_size = 3; sensor_msgs::msg::PointCloud2 output_cluster; - image_projection_based_fusion::closest_cluster( + autoware::image_projection_based_fusion::closest_cluster( pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); EXPECT_EQ(output_cluster.data.size(), std::size_t(0)); } @@ -96,7 +96,7 @@ TEST(UtilsTest, closest_cluster_test_case2) double cluster_2d_tolerance = 0.5; int min_cluster_size = 3; sensor_msgs::msg::PointCloud2 output_cluster; - image_projection_based_fusion::closest_cluster( + autoware::image_projection_based_fusion::closest_cluster( pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); EXPECT_EQ(output_cluster.data.size(), dummy_x.size() * pointcloud.point_step); } @@ -127,7 +127,7 @@ TEST(UtilsTest, closest_cluster_test_case3) double cluster_2d_tolerance = 0.5; int min_cluster_size = 3; sensor_msgs::msg::PointCloud2 output_cluster; - image_projection_based_fusion::closest_cluster( + autoware::image_projection_based_fusion::closest_cluster( pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); EXPECT_EQ(output_cluster.data.size(), 3 * pointcloud.point_step); } @@ -158,7 +158,7 @@ TEST(UtilsTest, closest_cluster_test_case4) double cluster_2d_tolerance = 0.5; int min_cluster_size = 3; sensor_msgs::msg::PointCloud2 output_cluster; - image_projection_based_fusion::closest_cluster( + autoware::image_projection_based_fusion::closest_cluster( pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); EXPECT_EQ(output_cluster.data.size(), 6 * pointcloud.point_step); } 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/config/transfusion.param.yaml b/perception/lidar_transfusion/config/transfusion.param.yaml index 2c6680fe50af1..af1f63c335501 100644 --- a/perception/lidar_transfusion/config/transfusion.param.yaml +++ b/perception/lidar_transfusion/config/transfusion.param.yaml @@ -3,6 +3,7 @@ # 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: [-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] diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp index 0ad3ab2231f50..0d0148d2f6c17 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp @@ -25,11 +25,13 @@ class TransfusionConfig { public: TransfusionConfig( - const std::vector & voxels_num, const std::vector & point_cloud_range, - const std::vector & voxel_size, const int num_proposals, - const float circle_nms_dist_threshold, const std::vector & yaw_norm_thresholds, - const float score_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]; @@ -86,7 +88,7 @@ class TransfusionConfig } ///// 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}; diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/lidar_transfusion/schema/transfusion.schema.json index 7debc0edda6fb..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": { diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp index 7f5833e60d6d0..a07e385208748 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -27,11 +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 int num_proposals = (this->declare_parameter("num_proposals", 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); @@ -74,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, num_proposals, 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/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 97d66bf9f648c..4200836181276 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -19,58 +19,14 @@ You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which mean ![image_for_frame_parameter_visualization](./image/gridmap_frame_settings.drawio.svg) -### Each config parameters - -Config parameters are managed in `config/*.yaml` and here shows its outline. - -- Pointcloud based occupancy grid map - -| Ros param name | Default value | -| -------------------------------------------- | ------------- | -| map_frame | "map" | -| base_link_frame | "base_link" | -| scan_origin_frame | "base_link" | -| gridmap_origin_frame | "base_link" | -| use_height_filter | true | -| enable_single_frame_mode | false | -| filter_obstacle_pointcloud_by_raw_pointcloud | false | -| map_length | 150.0 [m] | -| map_resolution | 0.5 [m] | -| use_projection | false | -| projection_dz_threshold | 0.01 | -| obstacle_separation_threshold | 1.0 | -| input_obstacle_pointcloud | true | -| input_obstacle_and_raw_pointcloud | true | - -- Laserscan based occupancy grid map - -| Ros param name | Default value | -| ------------------------ | ------------- | -| map_length | 150 [m] | -| map_width | 150 [m] | -| map_resolution | 0.5 [m] | -| use_height_filter | true | -| enable_single_frame_mode | false | -| map_frame | "map" | -| base_link_frame | "base_link" | -| scan_origin_frame | "base_link" | -| gridmap_origin_frame | "base_link" | - -## Other parameters - -Additional argument is shown below: - -| Name | Default | Description | -| ----------------------------------- | ------------------------------ | --------------------------------------------------------------------------------------------- | -| `use_multithread` | `false` | whether to use multithread | -| `use_intra_process` | `false` | | -| `map_origin` | `` | parameter to override `map_origin_frame` which means grid map origin | -| `scan_origin` | `` | parameter to override `scan_origin_frame` which means scanning center | -| `output` | `occupancy_grid` | output name | -| `use_pointcloud_container` | `false` | | -| `container_name` | `occupancy_grid_map_container` | | -| `input_obstacle_pointcloud` | `false` | only for laserscan based method. If true, the node subscribe obstacle pointcloud | -| `input_obstacle_and_raw_pointcloud` | `true` | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud | +### Parameters + +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json") }} ### Downsample input pointcloud(Optional) diff --git a/perception/probabilistic_occupancy_grid_map/config/grid_map_param.yaml b/perception/probabilistic_occupancy_grid_map/config/grid_map.param.yaml similarity index 100% rename from perception/probabilistic_occupancy_grid_map/config/grid_map_param.yaml rename to perception/probabilistic_occupancy_grid_map/config/grid_map.param.yaml diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index 2b2057af9cc12..09ad0a4d554a1 100644 --- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp @@ -52,8 +52,10 @@ #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 @@ -85,11 +87,52 @@ 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 diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp index 9dfd8b8689123..3301fd1987bd3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp @@ -36,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; diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp index 1f0f92d933ff3..7569a60b36466 100644 --- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp @@ -40,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; diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index 50b470d904ef6..a8288d2720f48 100644 --- a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -97,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, diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp index 5e9b5598c8f63..39855ec36260c 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp @@ -80,17 +80,21 @@ 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, 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; @@ -232,5 +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/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index dffbda18d0c8a..c8af97fcf3ba0 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -58,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 @@ -86,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(), diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index 39224bc7e3a69..f5f2c70ae6dfc 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -59,21 +59,20 @@ 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 @@ -100,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(), diff --git a/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp index e8c2cdb2617df..902de2148cffe 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -60,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, diff --git a/perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json b/perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json new file mode 100644 index 0000000000000..d087095ce980b --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json @@ -0,0 +1,61 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Binary Bayes Filter Updater", + "type": "object", + "definitions": { + "binary_bayes_filter_updater": { + "type": "object", + "properties": { + "probability_matrix": { + "type": "object", + "properties": { + "occupied_to_occupied": { + "type": "number", + "description": "Probability of transitioning from occupied to occupied state.", + "default": 0.95 + }, + "occupied_to_free": { + "type": "number", + "description": "Probability of transitioning from occupied to free state.", + "default": 0.05 + }, + "free_to_occupied": { + "type": "number", + "description": "Probability of transitioning from free to occupied state.", + "default": 0.2 + }, + "free_to_free": { + "type": "number", + "description": "Probability of transitioning from free to free state.", + "default": 0.8 + } + }, + "required": [ + "occupied_to_occupied", + "occupied_to_free", + "free_to_occupied", + "free_to_free" + ] + }, + "v_ratio": { + "type": "number", + "description": "Ratio of the variance used in the filter.", + "default": 0.1 + } + }, + "required": ["probability_matrix", "v_ratio"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/binary_bayes_filter_updater" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json b/perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json new file mode 100644 index 0000000000000..e874431ec2601 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/grid_map.schema.json @@ -0,0 +1,76 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Grid Map", + "type": "object", + "definitions": { + "visualization_params": { + "type": "object", + "properties": { + "type": { + "type": "string", + "description": "The type of grid map visualization.", + "default": "occupancy_grid" + }, + "params": { + "type": "object", + "properties": { + "layer": { + "type": "string", + "description": "The layer of the grid map visualization.", + "default": "filled_free_to_farthest" + }, + "data_min": { + "type": "number", + "description": "The minimum data value for the visualization.", + "default": 0.0 + }, + "data_max": { + "type": "number", + "description": "The maximum data value for the visualization.", + "default": 100.0 + } + }, + "required": ["layer", "data_min", "data_max"] + } + }, + "required": ["type", "params"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "grid_map_topic": { + "type": "string", + "description": "The topic name for the grid map.", + "default": "/perception/occupancy_grid_map/occupancy_grid_map_node/debug/grid_map" + }, + "grid_map_visualizations": { + "type": "array", + "description": "List of grid map visualizations.", + "items": { + "type": "string" + }, + "default": ["grid_1st_step", "grid_2nd_step", "grid_3rd_step"] + }, + "grid_1st_step": { "$ref": "#/definitions/visualization_params" }, + "grid_2nd_step": { "$ref": "#/definitions/visualization_params" }, + "grid_3rd_step": { "$ref": "#/definitions/visualization_params" } + }, + "required": [ + "grid_map_topic", + "grid_map_visualizations", + "grid_1st_step", + "grid_2nd_step", + "grid_3rd_step" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json b/perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json new file mode 100644 index 0000000000000..2b6befd1ae1fd --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json @@ -0,0 +1,94 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Laserscan Based Occupancy Grid Map", + "type": "object", + "definitions": { + "height_filter_params": { + "type": "object", + "properties": { + "use_height_filter": { + "type": "boolean", + "description": "Flag to use height filter.", + "default": true + }, + "min_height": { + "type": "number", + "description": "Minimum height for the height filter.", + "default": -1.0 + }, + "max_height": { + "type": "number", + "description": "Maximum height for the height filter.", + "default": 2.0 + } + }, + "required": ["use_height_filter", "min_height", "max_height"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "map_frame": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "gridmap_origin_frame": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "scan_origin_frame": { + "type": "string", + "description": "The frame ID of the scan origin.", + "default": "base_link" + }, + "height_filter": { "$ref": "#/definitions/height_filter_params" }, + "enable_single_frame_mode": { + "type": "boolean", + "description": "Flag to enable single frame mode.", + "default": false + }, + "map_length": { + "type": "number", + "description": "The length of the map.", + "default": 150.0 + }, + "map_width": { + "type": "number", + "description": "The width of the map.", + "default": 150.0 + }, + "map_resolution": { + "type": "number", + "description": "The resolution of the map.", + "default": 0.5 + } + }, + "required": [ + "map_frame", + "base_link_frame", + "gridmap_origin_frame", + "scan_origin_frame", + "height_filter", + "enable_single_frame_mode", + "map_length", + "map_width", + "map_resolution" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json b/perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json new file mode 100644 index 0000000000000..e522e486e88cc --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json @@ -0,0 +1,207 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Multi Lidar Pointcloud Based Occupancy Grid Map", + "type": "object", + "definitions": { + "height_filter_params": { + "type": "object", + "properties": { + "use_height_filter": { + "type": "boolean", + "description": "Flag to use height filter.", + "default": true + }, + "min_height": { + "type": "number", + "description": "Minimum height for the height filter.", + "default": -1.0 + }, + "max_height": { + "type": "number", + "description": "Maximum height for the height filter.", + "default": 2.0 + } + }, + "required": ["use_height_filter", "min_height", "max_height"] + }, + "occupancy_grid_map_fixed_blind_spot_params": { + "type": "object", + "properties": { + "distance_margin": { + "type": "number", + "description": "Distance margin for the fixed blind spot.", + "default": 1.0 + } + }, + "required": ["distance_margin"] + }, + "occupancy_grid_map_projective_blind_spot_params": { + "type": "object", + "properties": { + "projection_dz_threshold": { + "type": "number", + "description": "Projection dz threshold to avoid null division.", + "default": 0.01 + }, + "obstacle_separation_threshold": { + "type": "number", + "description": "Threshold to fill the interval between obstacles with unknown.", + "default": 1.0 + }, + "pub_debug_grid": { + "type": "boolean", + "description": "Flag to publish the debug grid.", + "default": false + } + }, + "required": ["projection_dz_threshold", "obstacle_separation_threshold", "pub_debug_grid"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "shared_config": { + "type": "object", + "properties": { + "map_frame": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "gridmap_origin_frame": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "map_resolution": { + "type": "number", + "description": "The resolution of the map.", + "default": 0.5 + }, + "map_length_x": { + "type": "number", + "description": "The length of the map in the x direction.", + "default": 150.0 + }, + "map_length_y": { + "type": "number", + "description": "The length of the map in the y direction.", + "default": 150.0 + } + }, + "required": [ + "map_frame", + "base_link_frame", + "gridmap_origin_frame", + "map_resolution", + "map_length_x", + "map_length_y" + ] + }, + "downsample_input_pointcloud": { + "type": "boolean", + "description": "Flag to downsample the input pointcloud.", + "default": true + }, + "downsample_voxel_size": { + "type": "number", + "description": "The voxel size for downsampling.", + "default": 0.25 + }, + "ogm_creation_config": { + "type": "object", + "properties": { + "height_filter": { "$ref": "#/definitions/height_filter_params" }, + "enable_single_frame_mode": { + "type": "boolean", + "description": "Flag to enable single frame mode.", + "default": true + }, + "filter_obstacle_pointcloud_by_raw_pointcloud": { + "type": "boolean", + "description": "Flag to filter obstacle pointcloud by raw pointcloud.", + "default": false + }, + "grid_map_type": { + "type": "string", + "description": "Type of the grid map.", + "default": "OccupancyGridMapFixedBlindSpot" + }, + "OccupancyGridMapFixedBlindSpot": { + "$ref": "#/definitions/occupancy_grid_map_fixed_blind_spot_params" + }, + "OccupancyGridMapProjectiveBlindSpot": { + "$ref": "#/definitions/occupancy_grid_map_projective_blind_spot_params" + } + }, + "required": [ + "height_filter", + "enable_single_frame_mode", + "filter_obstacle_pointcloud_by_raw_pointcloud", + "grid_map_type" + ] + }, + "fusion_config": { + "type": "object", + "properties": { + "raw_pointcloud_topics": { + "type": "array", + "description": "List of raw pointcloud topics.", + "items": { + "type": "string" + } + }, + "fusion_input_ogm_topics": { + "type": "array", + "description": "List of fusion input occupancy grid map topics.", + "items": { + "type": "string" + } + }, + "input_ogm_reliabilities": { + "type": "array", + "description": "Reliability of each sensor for fusion.", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + } + }, + "fusion_method": { + "type": "string", + "description": "Method for occupancy grid map fusion.", + "enum": ["overwrite", "log-odds", "dempster-shafer"], + "default": "overwrite" + } + }, + "required": [ + "raw_pointcloud_topics", + "fusion_input_ogm_topics", + "input_ogm_reliabilities", + "fusion_method" + ] + } + }, + "required": [ + "shared_config", + "downsample_input_pointcloud", + "downsample_voxel_size", + "ogm_creation_config", + "fusion_config" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json b/perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json new file mode 100644 index 0000000000000..e79c094820ec4 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json @@ -0,0 +1,150 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pointcloud Based Occupancy Grid Map", + "type": "object", + "definitions": { + "height_filter_params": { + "type": "object", + "properties": { + "use_height_filter": { + "type": "boolean", + "description": "Flag to use height filter.", + "default": true + }, + "min_height": { + "type": "number", + "description": "Minimum height for the height filter.", + "default": -1.0 + }, + "max_height": { + "type": "number", + "description": "Maximum height for the height filter.", + "default": 2.0 + } + }, + "required": ["use_height_filter", "min_height", "max_height"] + }, + "occupancy_grid_map_fixed_blind_spot_params": { + "type": "object", + "properties": { + "distance_margin": { + "type": "number", + "description": "Distance margin for the fixed blind spot.", + "default": 1.0 + } + }, + "required": ["distance_margin"] + }, + "occupancy_grid_map_projective_blind_spot_params": { + "type": "object", + "properties": { + "projection_dz_threshold": { + "type": "number", + "description": "Projection dz threshold to avoid null division.", + "default": 0.01 + }, + "obstacle_separation_threshold": { + "type": "number", + "description": "Threshold to fill the interval between obstacles with unknown.", + "default": 1.0 + }, + "pub_debug_grid": { + "type": "boolean", + "description": "Flag to publish the debug grid.", + "default": false + } + }, + "required": ["projection_dz_threshold", "obstacle_separation_threshold", "pub_debug_grid"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "map_length": { + "type": "number", + "description": "The length of the map.", + "default": 150.0 + }, + "map_resolution": { + "type": "number", + "description": "The resolution of the map.", + "default": 0.5 + }, + "height_filter": { "$ref": "#/definitions/height_filter_params" }, + "downsample_input_pointcloud": { + "type": "boolean", + "description": "Flag to downsample the input pointcloud.", + "default": true + }, + "downsample_voxel_size": { + "type": "number", + "description": "The voxel size for downsampling.", + "default": 0.25 + }, + "enable_single_frame_mode": { + "type": "boolean", + "description": "Flag to enable single frame mode.", + "default": false + }, + "filter_obstacle_pointcloud_by_raw_pointcloud": { + "type": "boolean", + "description": "Flag to filter obstacle pointcloud by raw pointcloud.", + "default": false + }, + "map_frame": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "gridmap_origin_frame": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "scan_origin_frame": { + "type": "string", + "description": "The frame ID of the scan origin.", + "default": "base_link" + }, + "grid_map_type": { + "type": "string", + "description": "Type of the grid map.", + "default": "OccupancyGridMapFixedBlindSpot" + }, + "OccupancyGridMapFixedBlindSpot": { + "$ref": "#/definitions/occupancy_grid_map_fixed_blind_spot_params" + }, + "OccupancyGridMapProjectiveBlindSpot": { + "$ref": "#/definitions/occupancy_grid_map_projective_blind_spot_params" + } + }, + "required": [ + "map_length", + "map_resolution", + "height_filter", + "downsample_input_pointcloud", + "downsample_voxel_size", + "enable_single_frame_mode", + "filter_obstacle_pointcloud_by_raw_pointcloud", + "map_frame", + "base_link_frame", + "gridmap_origin_frame", + "scan_origin_frame", + "grid_map_type" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json b/perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json new file mode 100644 index 0000000000000..14f4305f55de8 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json @@ -0,0 +1,105 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Synchronized Grid Map Fusion Node", + "type": "object", + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "fusion_input_ogm_topics": { + "type": "array", + "description": "List of fusion input occupancy grid map topics.", + "items": { + "type": "string" + }, + "default": ["topic1", "topic2"] + }, + "input_ogm_reliabilities": { + "type": "array", + "description": "Reliability of each sensor for fusion.", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + }, + "default": [0.8, 0.2] + }, + "fusion_method": { + "type": "string", + "description": "Method for occupancy grid map fusion.", + "enum": ["overwrite", "log-odds", "dempster-shafer"], + "default": "overwrite" + }, + "match_threshold_sec": { + "type": "number", + "description": "Time threshold for matching in seconds.", + "default": 0.01 + }, + "timeout_sec": { + "type": "number", + "description": "Timeout for synchronization in seconds.", + "default": 0.1 + }, + "input_offset_sec": { + "type": "array", + "description": "Offset for each input in seconds.", + "items": { + "type": "number" + }, + "default": [0.0, 0.0] + }, + "map_frame_": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame_": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "grid_map_origin_frame_": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "fusion_map_length_x": { + "type": "number", + "description": "The length of the fusion map in the x direction.", + "default": 100.0 + }, + "fusion_map_length_y": { + "type": "number", + "description": "The length of the fusion map in the y direction.", + "default": 100.0 + }, + "fusion_map_resolution": { + "type": "number", + "description": "The resolution of the fusion map.", + "default": 0.5 + } + }, + "required": [ + "fusion_input_ogm_topics", + "input_ogm_reliabilities", + "fusion_method", + "match_threshold_sec", + "timeout_sec", + "input_offset_sec", + "map_frame_", + "base_link_frame_", + "grid_map_origin_frame_", + "fusion_map_length_x", + "fusion_map_length_y", + "fusion_map_resolution" + ] + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} 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 3fe75263b3f28..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 @@ -37,6 +37,7 @@ #endif #include +#include #include #include @@ -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 diff --git a/perception/tensorrt_yolox/README.md b/perception/tensorrt_yolox/README.md index af88e73cc04a6..c97fe74a324e7 100644 --- a/perception/tensorrt_yolox/README.md +++ b/perception/tensorrt_yolox/README.md @@ -31,44 +31,8 @@ Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Se ## Parameters -### Core Parameters - -| Name | Type | Default Value | Description | -| ----------------- | ----- | ------------- | -------------------------------------------------------------------------------------- | -| `score_threshold` | float | 0.3 | If the objectness score is less than this value, the object is ignored in yolox layer. | -| `nms_threshold` | float | 0.7 | The IoU threshold for NMS method | - -**NOTE:** These two parameters are only valid for "plain" model (described later). - -### Node Parameters - -| Name | Type | Default Value | Description | -| -------------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `model_path` | string | "" | The onnx file name for yolox model | -| `model_name` | string | "" | The yolox model name:
"yolox-sPlus-T4-960x960-pseudo-finetune" for detection only, could reduce resource and processing_time
"yolox-sPlus-opt-pseudoV2-T4-960x960-T4-seg16cls" for multi-task including semantic segmentation | -| `label_path` | string | "" | The label file with label names for detected objects written on it | -| `precision` | string | "fp16" | The inference mode: "fp32", "fp16", "int8" | -| `build_only` | bool | false | shutdown node after TensorRT engine file is built | -| `calibration_algorithm` | string | "MinMax" | Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: Entropy",("Legacy" \| "Percentile"), "MinMax"] | -| `dla_core_id` | int | -1 | If positive ID value is specified, the node assign inference task to the DLA core | -| `quantize_first_layer` | bool | false | If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8 | -| `quantize_last_layer` | bool | false | If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8 | -| `profile_per_layer` | bool | false | If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose. | -| `clip_value` | double | 0.0 | If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration | -| `preprocess_on_gpu` | bool | true | If true, pre-processing is performed on GPU | -| `calibration_image_list_path` | string | "" | Path to a file which contains path to images. Those images will be used for int8 quantization. | -| `yolox_s_plus_opt_param_path` | string | "" | Path to parameter file | -| `is_publish_color_mask` | bool | false | If true, publish color mask for result visualization | -| `is_roi_overlap_segment` | bool | true | If true, overlay detected object roi onto semantic segmentation to avoid over-filtering pointcloud especially small size objects | -| `overlap_roi_score_threshold` | float | 0.3 | minimum existence_probability of detected roi considered to replace segmentation | -| `roi_overlay_segment_label.UNKNOWN` | bool | true | If true, unknown objects roi will be overlaid onto sematic segmentation mask. | -| `roi_overlay_segment_label.CAR` | bool | false | If true, car objects roi will be overlaid onto sematic segmentation mask. | -| `roi_overlay_segment_label.TRUCK` | bool | false | If true, truck objects roi will be overlaid onto sematic segmentation mask. | -| `roi_overlay_segment_label.BUS` | bool | false | If true, bus objects roi will be overlaid onto sematic segmentation mask. | -| `roi_overlay_segment_label.MOTORCYCLE` | bool | true | If true, motorcycle objects roi will be overlaid onto sematic segmentation mask. | -| `roi_overlay_segment_label.BICYCLE` | bool | true | If true, bicycle objects roi will be overlaid onto sematic segmentation mask. | -| `roi_overlay_segment_label.PEDESTRIAN` | bool | true | If true, pedestrian objects roi will be overlaid onto sematic segmentation mask. | -| `roi_overlay_segment_label.ANIMAL` | bool | true | If true, animal objects roi will be overlaid onto sematic segmentation mask. | +{{ json_to_markdown("perception/tensorrt_yolox/schema/yolox_s_plus_opt.schema.json") }} +{{ json_to_markdown("perception/tensorrt_yolox/schema/yolox_tiny.schema.json") }} ## Assumptions / Known limits diff --git a/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml b/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml index 57c1b40c44a4d..d963074e51840 100644 --- a/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml +++ b/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml @@ -1,6 +1,7 @@ # cspell: ignore semseg /**: ros__parameters: + # refine segmentation mask by overlay roi class # disable when sematic segmentation accuracy is good enough is_roi_overlap_segment: true @@ -26,6 +27,7 @@ color_map_path: "$(var data_path)/tensorrt_yolox/semseg_color_map.csv" score_threshold: 0.35 nms_threshold: 0.7 + precision: "int8" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]. calibration_algorithm: "Entropy" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]. dla_core_id: -1 # If positive ID value is specified, the node assign inference task to the DLA core. diff --git a/perception/tensorrt_yolox/config/yolox_tiny.param.yaml b/perception/tensorrt_yolox/config/yolox_tiny.param.yaml index e45742a7afb95..051aa22f52d7d 100644 --- a/perception/tensorrt_yolox/config/yolox_tiny.param.yaml +++ b/perception/tensorrt_yolox/config/yolox_tiny.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: - model_path: "$(var data_path)/tensorrt_yolox/$(var model_name).onnx" - label_path: "$(var data_path)/tensorrt_yolox/label.txt" - score_threshold: 0.35 - nms_threshold: 0.7 + model_path: "$(var data_path)/tensorrt_yolox/$(var model_name).onnx" # The onnx file name for YOLOX model. + label_path: "$(var data_path)/tensorrt_yolox/label.txt" # The label file path for YOLOX model. + score_threshold: 0.35 # Objects with a score lower than this value will be ignored. This threshold will be ignored if specified model contains EfficientNMS_TRT module in it. + nms_threshold: 0.7 # Detection results will be ignored if IoU over this value. This threshold will be ignored if specified model contains EfficientNMS_TRT module in it. precision: "fp16" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]. calibration_algorithm: "MinMax" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]. dla_core_id: -1 # If positive ID value is specified, the node assign inference task to the DLA core. diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index c13384918bd9c..f1189b53c23bf 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -40,64 +40,25 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) using std::placeholders::_1; using std::chrono_literals::operator""ms; - auto declare_parameter_with_description = - [this](std::string name, auto default_val, std::string description = "") { - auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; - param_desc.description = description; - return this->declare_parameter(name, default_val, param_desc); - }; - - std::string model_path = - declare_parameter_with_description("model_path", "", "The onnx file name for YOLOX model"); - std::string label_path = declare_parameter_with_description( - "label_path", "", - "The label file that consists of label name texts for detected object categories"); - std::string precision = declare_parameter_with_description( - "precision", "fp32", - "operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]"); - float score_threshold = declare_parameter_with_description( - "score_threshold", 0.3, - ("Objects with a score lower than this value will be ignored. " - "This threshold will be ignored if specified model contains EfficientNMS_TRT module in it")); - float nms_threshold = declare_parameter_with_description( - "nms_threshold", 0.7, - ("Detection results will be ignored if IoU over this value. " - "This threshold will be ignored if specified model contains EfficientNMS_TRT module in it")); - std::string calibration_algorithm = declare_parameter_with_description( - "calibration_algorithm", "MinMax", - ("Calibration algorithm to be used for quantization when precision==int8. " - "Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]")); - int dla_core_id = declare_parameter_with_description( - "dla_core_id", -1, - "If positive ID value is specified, the node assign inference task to the DLA core"); - bool quantize_first_layer = declare_parameter_with_description( - "quantize_first_layer", false, - ("If true, set the operating precision for the first (input) layer to be fp16. " - "This option is valid only when precision==int8")); - bool quantize_last_layer = declare_parameter_with_description( - "quantize_last_layer", false, - ("If true, set the operating precision for the last (output) layer to be fp16. " - "This option is valid only when precision==int8")); - bool profile_per_layer = declare_parameter_with_description( - "profile_per_layer", false, - ("If true, profiler function will be enabled. " - "Since the profile function may affect execution speed, it is recommended " - "to set this flag true only for development purpose.")); - double clip_value = declare_parameter_with_description( - "clip_value", 0.0, - ("If positive value is specified, " - "the value of each layer output will be clipped between [0.0, clip_value]. " - "This option is valid only when precision==int8 and used to manually specify " - "the dynamic range instead of using any calibration")); - bool preprocess_on_gpu = declare_parameter_with_description( - "preprocess_on_gpu", true, "If true, pre-processing is performed on GPU"); - std::string calibration_image_list_path = declare_parameter_with_description( - "calibration_image_list_path", "", - ("Path to a file which contains path to images." - "Those images will be used for int8 quantization.")); - - std::string color_map_path = declare_parameter_with_description( - "color_map_path", "", ("Path to a file which contains path to color map.")); + const std::string model_path = this->declare_parameter("model_path"); + const std::string label_path = this->declare_parameter("label_path"); + const std::string precision = this->declare_parameter("precision"); + const float score_threshold = + static_cast(this->declare_parameter("score_threshold")); + const float nms_threshold = static_cast(this->declare_parameter("nms_threshold")); + const std::string calibration_algorithm = + this->declare_parameter("calibration_algorithm"); + const int dla_core_id = this->declare_parameter("dla_core_id"); + const bool quantize_first_layer = this->declare_parameter("quantize_first_layer"); + const bool quantize_last_layer = this->declare_parameter("quantize_last_layer"); + const bool profile_per_layer = this->declare_parameter("profile_per_layer"); + const double clip_value = this->declare_parameter("clip_value"); + const bool preprocess_on_gpu = this->declare_parameter("preprocess_on_gpu"); + const std::string calibration_image_list_path = + this->declare_parameter("calibration_image_list_path"); + + std::string color_map_path = this->declare_parameter("color_map_path"); + if (!readLabelFile(label_path)) { RCLCPP_ERROR(this->get_logger(), "Could not find label file"); rclcpp::shutdown(); diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index 7d241d71ce185..c1fbd4a4ad4f2 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -187,7 +187,7 @@ bool MapBasedDetector::getTransform( geometry_msgs::msg::TransformStamped transform = tf_buffer_.lookupTransform("map", frame_id, t, rclcpp::Duration::from_seconds(0.2)); tf2::fromMsg(transform.transform, tf); - } catch (tf2::TransformException & ex) { + } catch (const tf2::TransformException & ex) { return false; } return true; diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp index 607aabdefa3cc..0e10fec3dbb87 100644 --- a/perception/traffic_light_multi_camera_fusion/src/node.cpp +++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp @@ -293,7 +293,7 @@ void MultiCameraFusion::groupFusion( std::map & grouped_record_map) { grouped_record_map.clear(); - for (auto & p : fused_record_map) { + for (const auto & p : fused_record_map) { IdType roi_id = p.second.roi.traffic_light_id; /* this should not happen diff --git a/perception/traffic_light_occlusion_predictor/CMakeLists.txt b/perception/traffic_light_occlusion_predictor/CMakeLists.txt index 481561ed92be8..61d8d97b815e6 100644 --- a/perception/traffic_light_occlusion_predictor/CMakeLists.txt +++ b/perception/traffic_light_occlusion_predictor/CMakeLists.txt @@ -13,18 +13,18 @@ include_directories( ${PCL_INCLUDE_DIRS} ) -ament_auto_add_library(traffic_light_occlusion_predictor SHARED - src/nodelet.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/node.cpp src/occlusion_predictor.cpp ) -rclcpp_components_register_node(traffic_light_occlusion_predictor - PLUGIN "traffic_light::TrafficLightOcclusionPredictorNodelet" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::TrafficLightOcclusionPredictorNode" EXECUTABLE traffic_light_occlusion_predictor_node ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(traffic_light_occlusion_predictor ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) ament_auto_package(INSTALL_TO_SHARE config diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/node.cpp similarity index 92% rename from perception/traffic_light_occlusion_predictor/src/nodelet.cpp rename to perception/traffic_light_occlusion_predictor/src/node.cpp index bc668c6af6c3c..8bc11fdea2aad 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/node.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_occlusion_predictor/nodelet.hpp" +#define EIGEN_MPL2_ONLY + +#include "node.hpp" +#include +#include #include #include #include @@ -30,16 +34,13 @@ #include #include #include +#include #include -#define EIGEN_MPL2_ONLY -#include -#include - -namespace traffic_light +namespace autoware::traffic_light { -TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( +TrafficLightOcclusionPredictorNode::TrafficLightOcclusionPredictorNode( const rclcpp::NodeOptions & node_options) : Node("traffic_light_occlusion_predictor_node", node_options), tf_buffer_(this->get_clock()), @@ -53,7 +54,7 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( // subscribers map_sub_ = create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&TrafficLightOcclusionPredictorNodelet::mapCallback, this, _1)); + std::bind(&TrafficLightOcclusionPredictorNode::mapCallback, this, _1)); // publishers signal_pub_ = @@ -79,7 +80,7 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( synchronizer_ = std::make_shared( this, topics, qos, std::bind( - &TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4, + &TrafficLightOcclusionPredictorNode::syncCallback, this, _1, _2, _3, _4, tier4_perception_msgs::msg::TrafficLightRoi::CAR_TRAFFIC_LIGHT), config_.max_image_cloud_delay, config_.max_wait_t); @@ -89,14 +90,14 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( synchronizer_ped_ = std::make_shared( this, topics_ped, qos_ped, std::bind( - &TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4, + &TrafficLightOcclusionPredictorNode::syncCallback, this, _1, _2, _3, _4, tier4_perception_msgs::msg::TrafficLightRoi::PEDESTRIAN_TRAFFIC_LIGHT), config_.max_image_cloud_delay, config_.max_wait_t); subscribed_.resize(2, false); } -void TrafficLightOcclusionPredictorNodelet::mapCallback( +void TrafficLightOcclusionPredictorNode::mapCallback( const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr input_msg) { traffic_light_position_map_.clear(); @@ -121,7 +122,7 @@ void TrafficLightOcclusionPredictorNodelet::mapCallback( } } -void TrafficLightOcclusionPredictorNodelet::syncCallback( +void TrafficLightOcclusionPredictorNode::syncCallback( const tier4_perception_msgs::msg::TrafficLightArray::ConstSharedPtr in_signal_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr in_cam_info_msg, @@ -184,7 +185,7 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( std::fill(subscribed_.begin(), subscribed_.end(), false); } } -} // namespace traffic_light +} // namespace autoware::traffic_light #include -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightOcclusionPredictorNodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightOcclusionPredictorNode) diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp b/perception/traffic_light_occlusion_predictor/src/node.hpp similarity index 88% rename from perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp rename to perception/traffic_light_occlusion_predictor/src/node.hpp index 3cd270adf9383..5ee4876a6abbf 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp +++ b/perception/traffic_light_occlusion_predictor/src/node.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__NODELET_HPP_ -#define TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__NODELET_HPP_ +#ifndef NODE_HPP_ +#define NODE_HPP_ + +#include "occlusion_predictor.hpp" #include #include -#include #include #include @@ -44,12 +45,12 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { -class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node +class TrafficLightOcclusionPredictorNode : public rclcpp::Node { public: - explicit TrafficLightOcclusionPredictorNodelet(const rclcpp::NodeOptions & node_options); + explicit TrafficLightOcclusionPredictorNode(const rclcpp::NodeOptions & node_options); private: struct Config @@ -108,5 +109,5 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node std::vector occlusion_ratios_; tier4_perception_msgs::msg::TrafficLightArray out_msg_; }; -} // namespace traffic_light -#endif // TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__NODELET_HPP_ +} // namespace autoware::traffic_light +#endif // NODE_HPP_ diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 40e50aabf3c17..74878ba0b93af 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -13,14 +13,15 @@ // limitations under the License. // -#include "traffic_light_occlusion_predictor/occlusion_predictor.hpp" +#include "occlusion_predictor.hpp" +#include namespace { -traffic_light::Ray point2ray(const pcl::PointXYZ & pt) +autoware::traffic_light::Ray point2ray(const pcl::PointXYZ & pt) { - traffic_light::Ray ray; + autoware::traffic_light::Ray ray; ray.dist = std::sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z); ray.elevation = RAD2DEG(std::atan2(pt.y, std::hypot(pt.x, pt.z))); ray.azimuth = RAD2DEG(std::atan2(pt.x, pt.z)); @@ -29,7 +30,7 @@ traffic_light::Ray point2ray(const pcl::PointXYZ & pt) } // namespace -namespace traffic_light +namespace autoware::traffic_light { CloudOcclusionPredictor::CloudOcclusionPredictor( @@ -253,4 +254,4 @@ uint32_t CloudOcclusionPredictor::predict( return 100 * occluded_num / tl_sample_cloud.size(); } -} // namespace traffic_light +} // namespace autoware::traffic_light diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.hpp similarity index 92% rename from perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp rename to perception/traffic_light_occlusion_predictor/src/occlusion_predictor.hpp index 066f438a9544f..435ee478eea22 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.hpp @@ -13,8 +13,8 @@ // limitations under the License. // -#ifndef TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__OCCLUSION_PREDICTOR_HPP_ -#define TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__OCCLUSION_PREDICTOR_HPP_ +#ifndef OCCLUSION_PREDICTOR_HPP_ +#define OCCLUSION_PREDICTOR_HPP_ #include #include @@ -43,7 +43,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { struct Ray @@ -93,6 +93,6 @@ class CloudOcclusionPredictor float elevation_occlusion_resolution_deg_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__OCCLUSION_PREDICTOR_HPP_ +#endif // OCCLUSION_PREDICTOR_HPP_ 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_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp index 62f9a55c75455..be9b2cfc1ccf6 100644 --- a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp @@ -59,10 +59,13 @@ class RTCInterface void removeExpiredCooperateStatus(); void clearCooperateStatus(); bool isActivated(const UUID & uuid) const; + bool isForceActivated(const UUID & uuid) const; bool isRegistered(const UUID & uuid) const; bool isRTCEnabled(const UUID & uuid) const; + bool isTerminated(const UUID & uuid) const; void lockCommandUpdate(); void unlockCommandUpdate(); + void print() const; private: void onCooperateCommandService( diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 985e38b64f2bd..1352ec10557f9 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -20,7 +20,7 @@ namespace { using tier4_rtc_msgs::msg::Module; -std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & uuid) { std::stringstream ss; for (auto i = 0; i < 16; ++i) { @@ -29,6 +29,34 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) return ss.str(); } +std::string command_to_string(const uint8_t type) +{ + if (type == tier4_rtc_msgs::msg::Command::ACTIVATE) { + return "ACTIVATE"; + } else if (type == tier4_rtc_msgs::msg::Command::DEACTIVATE) { + return "DEACTIVATE"; + } + + throw std::domain_error("invalid rtc command."); +} + +std::string state_to_string(const uint8_t type) +{ + if (type == tier4_rtc_msgs::msg::State::WAITING_FOR_EXECUTION) { + return "WAITING_FOR_EXECUTION"; + } else if (type == tier4_rtc_msgs::msg::State::RUNNING) { + return "RUNNING"; + } else if (type == tier4_rtc_msgs::msg::State::ABORTING) { + return "ABORTING"; + } else if (type == tier4_rtc_msgs::msg::State::SUCCEEDED) { + return "SUCCEEDED"; + } else if (type == tier4_rtc_msgs::msg::State::FAILED) { + return "FAILED"; + } + + throw std::domain_error("invalid rtc state."); +} + Module getModuleType(const std::string & module_name) { Module module; @@ -158,14 +186,14 @@ std::vector RTCInterface::validateCooperateCommands( } else { RCLCPP_WARN_STREAM( getLogger(), "[validateCooperateCommands] uuid : " - << to_string(command.uuid) + << uuid_to_string(command.uuid) << " state is not WAITING_FOR_EXECUTION or RUNNING. state : " << itr->state.type << std::endl); response.success = false; } } else { RCLCPP_WARN_STREAM( - getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid) + getLogger(), "[validateCooperateCommands] uuid : " << uuid_to_string(command.uuid) << " is not found." << std::endl); response.success = false; } @@ -262,7 +290,7 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid) RCLCPP_WARN_STREAM( getLogger(), - "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl); + "[removeCooperateStatus] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); } void RTCInterface::removeStoredCommand(const UUID & uuid) @@ -313,7 +341,31 @@ bool RTCInterface::isActivated(const UUID & uuid) const } RCLCPP_WARN_STREAM( - getLogger(), "[isActivated] uuid : " << to_string(uuid) << " is not found." << std::endl); + getLogger(), "[isActivated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); + return false; +} + +bool RTCInterface::isForceActivated(const UUID & uuid) const +{ + std::lock_guard lock(mutex_); + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](const auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + if (itr->state.type != State::WAITING_FOR_EXECUTION && itr->state.type != State::RUNNING) { + return false; + } + if (itr->command_status.type == Command::ACTIVATE && !itr->safe) { + return true; + } else { + return false; + } + } + + RCLCPP_WARN_STREAM( + getLogger(), + "[isForceActivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl); return false; } @@ -338,7 +390,23 @@ bool RTCInterface::isRTCEnabled(const UUID & uuid) const } RCLCPP_WARN_STREAM( - getLogger(), "[isRTCEnabled] uuid : " << to_string(uuid) << " is not found." << std::endl); + getLogger(), "[isRTCEnabled] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); + return is_auto_mode_enabled_; +} + +bool RTCInterface::isTerminated(const UUID & uuid) const +{ + std::lock_guard lock(mutex_); + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + return itr->state.type == State::SUCCEEDED || itr->state.type == State::FAILED; + } + + RCLCPP_WARN_STREAM( + getLogger(), "[isTerminated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl); return is_auto_mode_enabled_; } @@ -363,4 +431,16 @@ bool RTCInterface::isLocked() const return is_locked_; } +void RTCInterface::print() const +{ + RCLCPP_INFO_STREAM(getLogger(), "---print rtc cooperate statuses---" << std::endl); + for (const auto status : registered_status_.statuses) { + RCLCPP_INFO_STREAM( + getLogger(), "uuid:" << uuid_to_string(status.uuid) + << " command:" << command_to_string(status.command_status.type) + << std::boolalpha << " safe:" << status.safe + << " state:" << state_to_string(status.state.type) << std::endl); + } +} + } // namespace autoware::rtc_interface diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 557c0c871583d..0189bf554676b 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -73,13 +73,12 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_route_; - autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr - sub_odom_; + universe_utils::InterProcessPollingSubscriber< + nav_msgs::msg::Odometry, autoware::universe_utils::polling_policy::All>::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_lane_driving_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_trajectory_; - autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr - sub_parking_state_; + universe_utils::InterProcessPollingSubscriber::SharedPtr sub_parking_state_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp index e7fd51b30e658..be60f6249e93f 100644 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ b/planning/autoware_static_centerline_generator/src/utils.hpp @@ -39,8 +39,8 @@ geometry_msgs::msg::Pose get_center_pose( PathWithLaneId get_path_with_lane_id( const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, - const geometry_msgs::msg::Pose & start_pose, const double nearest_ego_dist_threshold, - const double nearest_ego_yaw_threshold); + const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, + const double ego_nearest_yaw_threshold); void update_centerline( lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 9c899c2226497..cac609f882272 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -506,7 +506,7 @@ std::optional SurroundObstacleCheckerNode: try { transform_stamped = tf_buffer_.lookupTransform(source, target, stamp, tf2::durationFromSec(duration_sec)); - } catch (tf2::TransformException & ex) { + } catch (const tf2::TransformException & ex) { return {}; } diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index e0bcf771948ca..b559e159a78ec 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -103,7 +103,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node const std::string & source, const std::string & target, const rclcpp::Time & stamp, double duration_sec) const; - bool isStopRequired(const bool is_obstacle_found, const bool is_stopped); + bool isStopRequired(const bool is_obstacle_found, const bool is_vehicle_stopped); // ros mutable tf2_ros::Buffer tf_buffer_{get_clock()}; 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..5d76f1d8e4edf 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" @@ -92,7 +93,8 @@ class VelocitySmootherNode : public rclcpp::Node this, "/localization/kinematic_state"}; autoware::universe_utils::InterProcessPollingSubscriber sub_current_acceleration_{this, "~/input/acceleration"}; - autoware::universe_utils::InterProcessPollingSubscriber + autoware::universe_utils::InterProcessPollingSubscriber< + VelocityLimit, universe_utils::polling_policy::Newest> sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"}; autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ this, "~/input/operation_mode_state"}; @@ -260,6 +262,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 +279,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 b881b68798cdc..0249cfa70b496 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_.takeNewData(); + external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData(); const auto operation_mode_ptr = sub_operation_mode_.takeData(); if (operation_mode_ptr) { operation_mode_ = *operation_mode_ptr; @@ -502,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; } @@ -519,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 @@ -566,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 } @@ -672,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); @@ -734,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); @@ -817,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; @@ -863,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; } @@ -902,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/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 501554b0a2b02..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 @@ -134,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); @@ -274,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/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_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 9df19c4de6834..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 @@ -53,7 +54,8 @@ class LaneChangeBase : lane_change_parameters_{std::move(parameters)}, common_data_ptr_{std::make_shared()}, direction_{direction}, - type_{type} + type_{type}, + time_keeper_(std::make_shared()) { } @@ -63,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; @@ -137,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_; } @@ -161,12 +170,27 @@ class LaneChangeBase 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; } void toStopState() { current_lane_change_state_ = LaneChangeStates::Stop; } @@ -204,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( @@ -252,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 2fdf7c6b550a3..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 @@ -183,6 +183,13 @@ struct PhaseInfo } }; +struct Lanes +{ + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + struct Info { PhaseInfo longitudinal_acceleration{0.0, 0.0}; @@ -190,9 +197,6 @@ struct Info PhaseInfo duration{0.0, 0.0}; PhaseInfo length{0.0, 0.0}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; - Pose lane_changing_start{}; Pose lane_changing_end{}; @@ -225,23 +229,26 @@ struct LanesPolygon { std::optional current; std::optional target; - std::vector target_backward; + std::optional expanded_target; + lanelet::BasicPolygon2d target_neighbor; + std::vector preceding_target; }; -struct Lanes -{ - lanelet::ConstLanelets current; - lanelet::ConstLanelets target; - 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 { - std::shared_ptr route_handler_ptr; + RouteHandlerPtr route_handler_ptr; Odometry::ConstSharedPtr self_odometry_ptr; - std::shared_ptr bpp_param_ptr; - std::shared_ptr lc_param_ptr; - Lanes lanes; + 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; } @@ -259,12 +266,7 @@ struct CommonData return std::hypot(x, y); } }; - -using RouteHandlerPtr = std::shared_ptr; -using BppParamPtr = std::shared_ptr; -using LCParamPtr = std::shared_ptr; using CommonDataPtr = std::shared_ptr; -using LanesPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change 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/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 97b5c621deea5..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{}; - LaneChangeInfo info; - bool is_safe{false}; + 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 8cf67f8cfc66f..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); @@ -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,8 +1549,6 @@ 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; @@ -1549,8 +1577,8 @@ 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_lat("Reject: failed to generate candidate path!!"); @@ -1592,7 +1620,7 @@ bool NormalLaneChange::getLaneChangePaths( } 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_lat("Ego is stopping near traffic light. Do not allow lane change"); continue; @@ -1600,10 +1628,10 @@ bool NormalLaneChange::getLaneChangePaths( 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)) { + !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."); @@ -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 ead047f839f47..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; } @@ -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/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index 3c62cc241b323..11f9d9f140214 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -88,10 +88,12 @@ class BehaviorPathPlannerNode : public rclcpp::Node private: // subscriber - autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ - this, "~/input/route", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ - this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletRoute, universe_utils::polling_policy::Newest> + route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletMapBin, universe_utils::polling_policy::Newest> + vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; autoware::universe_utils::InterProcessPollingSubscriber velocity_subscriber_{ this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber 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..3d5ade9faf927 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 @@ -217,7 +218,7 @@ void BehaviorPathPlannerNode::takeData() { // route { - const auto msg = route_subscriber_.takeNewData(); + const auto msg = route_subscriber_.takeData(); if (msg) { if (msg->segments.empty()) { RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); @@ -229,7 +230,7 @@ void BehaviorPathPlannerNode::takeData() } // map { - const auto msg = vector_map_subscriber_.takeNewData(); + const auto msg = vector_map_subscriber_.takeData(); if (msg) { map_ptr_ = msg; has_received_map_ = true; 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 */ @@ -331,8 +335,15 @@ class SceneModuleInterface { return std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { - return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && - rtc.second->isActivated(uuid_map_.at(rtc.first)); + if (!rtc.second->isRegistered(uuid_map_.at(rtc.first))) { + return false; + } + + if (rtc.second->isTerminated(uuid_map_.at(rtc.first))) { + return true; + } + + return rtc.second->isActivated(uuid_map_.at(rtc.first)); }); } @@ -341,7 +352,8 @@ class SceneModuleInterface return std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && - !rtc.second->isActivated(uuid_map_.at(rtc.first)); + !rtc.second->isActivated(uuid_map_.at(rtc.first)) && + !rtc.second->isTerminated(uuid_map_.at(rtc.first)); }); } @@ -641,6 +653,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/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_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_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index 8e20cf2c9f660..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) 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 6bd5e0faf1938..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 @@ -160,6 +160,10 @@ 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 079928a58bd9f..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 @@ -191,6 +191,9 @@ 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 std::vector wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"}; double wait_and_see_th_closest_distance{0.0}; 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 63985e574f3b2..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,9 @@ 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( 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 0cea2a4e633c3..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 @@ -154,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 34d06a46d9ac8..87a4c91792649 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 @@ -118,8 +118,16 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const double start_distance = autoware::motion_utils::calcSignedArcLength( path.points, ego_idx, left_shift.start_pose.position); const double finish_distance = start_distance + left_shift.relative_longitudinal; - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); + + // If force activated keep safety to false + if (rtc_interface_ptr_map_.at("left")->isForceActivated(left_shift.uuid)) { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, false, State::RUNNING, start_distance, finish_distance, clock_->now()); + } else { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); + } + if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, @@ -131,8 +139,15 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const double start_distance = autoware::motion_utils::calcSignedArcLength( path.points, ego_idx, right_shift.start_pose.position); const double finish_distance = start_distance + right_shift.relative_longitudinal; - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); + + if (rtc_interface_ptr_map_.at("right")->isForceActivated(right_shift.uuid)) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, false, State::RUNNING, start_distance, finish_distance, clock_->now()); + } else { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); + } + if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, 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 a3b1b7e10b885..600c352123833 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 @@ -43,7 +43,7 @@ double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); bool isWithinLanes( - const lanelet::ConstLanelets & lanelets, std::shared_ptr & planner_data); + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length); 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 246b96ec5440e..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 @@ -812,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": [ @@ -823,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 e1f8bd4ad2734..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 @@ -456,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); @@ -466,6 +466,77 @@ 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; 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 c5f338e91acbd..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 @@ -150,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 641ae9547b712..d1b78dbd6a00a 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()); @@ -314,6 +316,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + 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; @@ -368,6 +371,7 @@ 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; @@ -384,6 +388,7 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & ob 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; @@ -425,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."); @@ -478,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_; /** @@ -530,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); /** @@ -571,6 +579,45 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( return; } + auto candidate_sl_force_activated = [&](const std::string & direction) { + // If statement to avoid unnecessary warning occurring from isForceActivated function + if (candidate_uuid_ == uuid_map_.at(direction)) { + if (rtc_interface_ptr_map_.at(direction)->isForceActivated(candidate_uuid_)) { + return true; + } + } + return false; + }; + + auto registered_sl_force_activated = + [&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) { + return std::any_of( + shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) { + return rtc_interface_ptr_map_.at(direction)->isForceActivated(shift_line.uuid); + }); + }; + + /** + * Check if the candidate avoidance path is force activated + */ + if (candidate_sl_force_activated("left") || candidate_sl_force_activated("right")) { + data.yield_required = false; + data.safe_shift_line = data.new_shift_line; + return; + } + + /** + * Check if any registered shift line is force activated + */ + if ( + registered_sl_force_activated("left", left_shift_array_) || + registered_sl_force_activated("right", right_shift_array_)) { + data.yield_required = false; + data.safe_shift_line = data.new_shift_line; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe but force executed"); + return; + } + /** * If the yield maneuver is disabled, use unapproved_new_sl for avoidance path generation even if * the shift line is unsafe. @@ -624,6 +671,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; } @@ -704,6 +752,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) { @@ -812,6 +861,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 = [&]() { @@ -874,6 +924,7 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; resetPathCandidate(); @@ -1031,6 +1082,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; CandidateOutput output; @@ -1070,6 +1122,7 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const BehaviorModuleOutput StaticObstacleAvoidanceModule::planWaitingApproval() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); BehaviorModuleOutput out = plan(); if (path_shifter_.getShiftLines().empty()) { @@ -1126,6 +1179,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(); @@ -1192,6 +1246,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; } @@ -1267,6 +1322,7 @@ bool StaticObstacleAvoidanceModule::isValidShiftLine( void StaticObstacleAvoidanceModule::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); using utils::static_obstacle_avoidance::toShiftedPath; helper_->setData(planner_data_); @@ -1351,6 +1407,7 @@ void StaticObstacleAvoidanceModule::initRTCStatus() void StaticObstacleAvoidanceModule::updateRTCData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & data = avoid_data_; updateRegisteredRTCStatus(helper_->getPreviousSplineShiftPath().path); @@ -1386,6 +1443,8 @@ 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; @@ -1393,11 +1452,16 @@ void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData 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_); @@ -1422,6 +1486,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; @@ -1463,6 +1528,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) { @@ -1540,6 +1606,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. @@ -1588,6 +1655,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) { @@ -1635,6 +1703,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. @@ -1747,6 +1816,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 c3a67eb074d73..5e58466fa4229 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 @@ -239,10 +239,18 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; + const auto is_on_path = [this](const auto & object) { + const auto [overhang, point] = object.overhang_points.front(); + return std::abs(overhang) < 0.5 * data_->parameters.vehicle_width; + }; + const auto is_valid_shift_line = [](const auto & s) { return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; }; + ObjectDataArray unavoidable_objects; + + // target objects are sorted by longitudinal distance. AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin.has_value()) { @@ -253,22 +261,22 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } else { o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; } - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } - const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(o); const auto desire_shift_length = - helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value()); - if (utils::static_obstacle_avoidance::isSameDirectionShift( - is_object_on_right, desire_shift_length)) { + helper_->getShiftLength(o, isOnRight(o), o.avoid_margin.value()); + if (utils::static_obstacle_avoidance::isSameDirectionShift(isOnRight(o), desire_shift_length)) { o.info = ObjectInfo::SAME_DIRECTION_SHIFT; - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } @@ -276,13 +284,25 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); if (!feasible_shift_profile.has_value()) { - if (o.avoid_required && is_forward_object(o)) { + if (o.avoid_required && is_forward_object(o) && is_on_path(o)) { break; } else { + unavoidable_objects.push_back(o); continue; } } + // If there is an object that cannot be avoided, this module only avoids object on the same side + // as unavoidable object. + if (!unavoidable_objects.empty()) { + if (isOnRight(unavoidable_objects.front()) && !isOnRight(o)) { + break; + } + if (!isOnRight(unavoidable_objects.front()) && isOnRight(o)) { + break; + } + } + // use absolute dist for return-to-center, relative dist from current for avoiding. const auto feasible_return_distance = helper_->getMaxAvoidanceDistance(feasible_shift_profile.value().first); 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 75eb6ab0fb62e..40373f00d4620 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 @@ -339,6 +339,32 @@ bool isWithinIntersection( 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) { if (boost::geometry::within( @@ -708,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."); @@ -852,7 +886,7 @@ bool isSatisfiedWithNonVehicleCondition( } ObjectData::Behavior getObjectBehavior( - ObjectData & object, const std::shared_ptr & parameters) + const ObjectData & object, const std::shared_ptr & parameters) { if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { return ObjectData::Behavior::NONE; @@ -1067,7 +1101,7 @@ double calcShiftLength( } bool isWithinLanes( - const lanelet::ConstLanelets & lanelets, std::shared_ptr & planner_data) + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data) { const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; @@ -1842,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 = @@ -1869,6 +1905,8 @@ void filterTargetObjects( continue; } } else { + // TARGET: PEDESTRIAN, BICYCLE + o.is_within_intersection = filtering_utils::isWithinIntersection(o, planner_data->route_handler); o.is_parked = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md index 60601299daf8b..3cf62b92bb659 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md @@ -8,6 +8,41 @@ This module judges whether the ego should stop in front of the crosswalk in orde ![crosswalk_module](docs/crosswalk_module.svg){width=1100} +## Flowchart + +```plantuml +@startuml +skinparam monochrome true + +title modifyPathVelocity +start +:getPathEndPointsOnCrosswalk; +group apply slow down + :applySlowDownByLanleet2Map; + :applySlowDownByOcclusion; +end group +group calculate stop pose + :getDefaultStopPose; + :resamplePath; + :checkStopForCrosswalkUsers; + :checkStopForStuckVehicles; +end group +group apply stop + :getNearestStopFactor; + :setSafe; + :setDistanceToStop; + + if (isActivated() is True?) then (yes) + :planGo; + else (no) + :planStop; + endif +end group + +stop +@enduml +``` + ## Features ### Yield the Way to the Pedestrians diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp index bcf31eadcc35f..260ba58fd7d93 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp @@ -97,13 +97,14 @@ std::set getCrosswalkIdSetOnPath( bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr); -std::vector getPolygonIntersects( +std::optional> +getPathEndPointsOnCrosswalk( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, - const geometry_msgs::msg::Point & ego_pos, const size_t max_num); + const geometry_msgs::msg::Point & ego_pos); std::vector getLinestringIntersects( const PathWithLaneId & ego_path, const lanelet::BasicLineString2d & linestring, - const geometry_msgs::msg::Point & ego_pos, const size_t max_num); + const geometry_msgs::msg::Point & ego_pos); std::optional getStopLineFromMap( const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, 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..4308ce5d054ff 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 @@ -114,18 +114,20 @@ void sortCrosswalksByDistance( lanelet::ConstLanelets & crosswalks) { const auto compare = [&](const lanelet::ConstLanelet & l1, const lanelet::ConstLanelet & l2) { - const auto l1_intersects = - getPolygonIntersects(ego_path, l1.polygon2d().basicPolygon(), ego_pos, 2); - const auto l2_intersects = - getPolygonIntersects(ego_path, l2.polygon2d().basicPolygon(), ego_pos, 2); + const auto l1_end_points_on_crosswalk = + getPathEndPointsOnCrosswalk(ego_path, l1.polygon2d().basicPolygon(), ego_pos); + const auto l2_end_points_on_crosswalk = + getPathEndPointsOnCrosswalk(ego_path, l2.polygon2d().basicPolygon(), ego_pos); - if (l1_intersects.empty() || l2_intersects.empty()) { + if (!l1_end_points_on_crosswalk || !l2_end_points_on_crosswalk) { return true; } - const auto dist_l1 = calcSignedArcLength(ego_path.points, size_t(0), l1_intersects.front()); + const auto dist_l1 = + calcSignedArcLength(ego_path.points, size_t(0), l1_end_points_on_crosswalk->first); - const auto dist_l2 = calcSignedArcLength(ego_path.points, size_t(0), l2_intersects.front()); + const auto dist_l2 = + calcSignedArcLength(ego_path.points, size_t(0), l2_end_points_on_crosswalk->first); return dist_l1 < dist_l2; }; @@ -225,67 +227,23 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto recordTime(1); // Calculate intersection between path and crosswalks - const auto path_intersects = - getPolygonIntersects(*path, crosswalk_.polygon2d().basicPolygon(), ego_pos, 2); + const auto path_end_points_on_crosswalk = + getPathEndPointsOnCrosswalk(*path, crosswalk_.polygon2d().basicPolygon(), ego_pos); + if (!path_end_points_on_crosswalk) { + return {}; + } + const auto & first_path_point_on_crosswalk = path_end_points_on_crosswalk->first; + const auto & last_path_point_on_crosswalk = path_end_points_on_crosswalk->second; // Apply safety slow down speed if defined in Lanelet2 map - if (crosswalk_.hasAttribute("safety_slow_down_speed")) { - applySafetySlowDownSpeed( - *path, path_intersects, - static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); - } - // Apply safety slow down speed if the crosswalk is occluded - const auto now = clock_->now(); - const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) { - return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer); - }; - const auto crosswalk_has_traffic_light = - !crosswalk_.regulatoryElementsAs().empty(); - const auto is_crosswalk_ignored = - (planner_param_.occlusion_ignore_with_traffic_light && crosswalk_has_traffic_light) || - crosswalk_.hasAttribute("skip_occluded_slowdown"); - if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) { - const auto dist_ego_to_crosswalk = - calcSignedArcLength(path->points, ego_pos, path_intersects.front()); - const auto detection_range = - planner_data_->vehicle_info_.max_lateral_offset_m + - calculate_detection_range( - planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk, - planner_data_->current_velocity->twist.linear.x); - const auto is_ego_on_the_crosswalk = - dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m; - if (!is_ego_on_the_crosswalk) { - if (is_crosswalk_occluded( - crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), detection_range, - objects_ptr->objects, planner_param_)) { - if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = now; - if (cmp_with_time_buffer(current_initial_occlusion_time_, std::greater_equal{})) - most_recent_occlusion_time_ = now; - } else if (!cmp_with_time_buffer(most_recent_occlusion_time_, std::greater{})) { - current_initial_occlusion_time_.reset(); - } + applySlowDownByLanelet2Map(*path, first_path_point_on_crosswalk, last_path_point_on_crosswalk); - if (cmp_with_time_buffer(most_recent_occlusion_time_, std::less_equal{})) { - const auto target_velocity = calcTargetVelocity(path_intersects.front(), *path); - applySafetySlowDownSpeed( - *path, path_intersects, - std::max(target_velocity, planner_param_.occlusion_slow_down_velocity)); - debug_data_.virtual_wall_suffix = " (occluded)"; - } else { - most_recent_occlusion_time_.reset(); - } - } - } + // Apply safety slow down speed if the crosswalk is occluded + applySlowDownByOcclusion(*path, first_path_point_on_crosswalk, last_path_point_on_crosswalk); recordTime(2); // Calculate stop point with margin - const auto p_stop_line = getStopPointWithMargin(*path, path_intersects); - - std::optional default_stop_pose = std::nullopt; - if (p_stop_line.has_value()) { - default_stop_pose = - calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second); - } + const auto default_stop_pose = getDefaultStopPose(*path, first_path_point_on_crosswalk); // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -294,11 +252,13 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Decide to stop for crosswalk users const auto stop_factor_for_crosswalk_users = checkStopForCrosswalkUsers( - *path, sparse_resample_path, p_stop_line, path_intersects, default_stop_pose); + *path, sparse_resample_path, first_path_point_on_crosswalk, last_path_point_on_crosswalk, + default_stop_pose); // Decide to stop for stuck vehicle const auto stop_factor_for_stuck_vehicles = checkStopForStuckVehicles( - sparse_resample_path, objects_ptr->objects, path_intersects, default_stop_pose); + sparse_resample_path, objects_ptr->objects, first_path_point_on_crosswalk, + last_path_point_on_crosswalk, default_stop_pose); // Get nearest stop factor const auto nearest_stop_factor = @@ -328,35 +288,32 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } // NOTE: The stop point will be the returned point with the margin. -std::optional> CrosswalkModule::getStopPointWithMargin( +std::optional CrosswalkModule::getDefaultStopPose( const PathWithLaneId & ego_path, - const std::vector & path_intersects) const + const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const { const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; // If stop lines are found in the LL2 map. for (const auto & stop_line : stop_lines_) { - const auto p_stop_lines = getLinestringIntersects( - ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); + const auto p_stop_lines = + getLinestringIntersects(ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos); if (!p_stop_lines.empty()) { - return std::make_pair(p_stop_lines.front(), -base_link2front); + return calcLongitudinalOffsetPose(ego_path.points, p_stop_lines.front(), -base_link2front); } } // If stop lines are not found in the LL2 map. - if (!path_intersects.empty()) { - return std::make_pair( - path_intersects.front(), -planner_param_.stop_distance_from_crosswalk - base_link2front); - } - - return {}; + return calcLongitudinalOffsetPose( + ego_path.points, first_path_point_on_crosswalk, + -planner_param_.stop_distance_from_crosswalk - base_link2front); } std::optional CrosswalkModule::checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, - const std::optional> & p_stop_line, - const std::vector & path_intersects, + const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk, const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; @@ -367,10 +324,11 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( findEgoPassageDirectionAlongPath(sparse_resample_path); const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = - calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; + calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position); // Calculate attention range for crosswalk - const auto crosswalk_attention_range = getAttentionRange(sparse_resample_path, path_intersects); + const auto crosswalk_attention_range = getAttentionRange( + sparse_resample_path, first_path_point_on_crosswalk, last_path_point_on_crosswalk); // Get attention area, which is ego's footprints on the crosswalk const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range); @@ -381,20 +339,15 @@ 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 = - autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, - p.min_jerk_for_no_stop_decision); - const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; - if ( - dist_ego2crosswalk - base_link2front - braking_distance < - p.max_offset_to_crosswalk_for_yield) { - return {}; - } + const double dist_ego2crosswalk = + calcSignedArcLength(ego_path.points, ego_pos, first_path_point_on_crosswalk); + const auto braking_distance_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, + p.min_jerk_for_no_stop_decision); + const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; + if ( + dist_ego2crosswalk - base_link2front - braking_distance < p.max_offset_to_crosswalk_for_yield) { + return {}; } // Check pedestrian for stop @@ -462,20 +415,17 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( } std::pair CrosswalkModule::getAttentionRange( - const PathWithLaneId & ego_path, const std::vector & path_intersects) + const PathWithLaneId & ego_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk) { stop_watch_.tic(__func__); - if (path_intersects.size() < 2) { - return std::make_pair(0.0, 0.0); - } - const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto near_attention_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()) - + calcSignedArcLength(ego_path.points, ego_pos, first_path_point_on_crosswalk) - planner_param_.crosswalk_attention_range; const auto far_attention_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()) + + calcSignedArcLength(ego_path.points, ego_pos, last_path_point_on_crosswalk) + planner_param_.crosswalk_attention_range; const auto [clamped_near_attention_range, clamped_far_attention_range] = @@ -599,14 +549,14 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal auto reverse_ego_path = ego_path; std::reverse(reverse_ego_path.points.begin(), reverse_ego_path.points.end()); - const auto prev_crosswalk_intersects = getPolygonIntersects( - reverse_ego_path, prev_crosswalk->polygon2d().basicPolygon(), ego_pos, 2); - if (prev_crosswalk_intersects.empty()) { + const auto path_end_points_on_prev_crosswalk = getPathEndPointsOnCrosswalk( + reverse_ego_path, prev_crosswalk->polygon2d().basicPolygon(), ego_pos); + if (!path_end_points_on_prev_crosswalk) { return near_attention_range; } const auto dist_to_prev_crosswalk = - calcSignedArcLength(ego_path.points, ego_pos, prev_crosswalk_intersects.front()); + calcSignedArcLength(ego_path.points, ego_pos, path_end_points_on_prev_crosswalk->first); return std::max(near_attention_range, dist_to_prev_crosswalk); }(); @@ -614,15 +564,14 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal if (!next_crosswalk) { return far_attention_range; } - const auto next_crosswalk_intersects = - getPolygonIntersects(ego_path, next_crosswalk->polygon2d().basicPolygon(), ego_pos, 2); - - if (next_crosswalk_intersects.empty()) { + const auto path_end_points_on_next_crosswalk = + getPathEndPointsOnCrosswalk(ego_path, next_crosswalk->polygon2d().basicPolygon(), ego_pos); + if (!path_end_points_on_next_crosswalk) { return far_attention_range; } const auto dist_to_next_crosswalk = - calcSignedArcLength(ego_path.points, ego_pos, next_crosswalk_intersects.front()); + calcSignedArcLength(ego_path.points, ego_pos, path_end_points_on_next_crosswalk->first); return std::min(far_attention_range, dist_to_next_crosswalk); }(); @@ -830,14 +779,11 @@ CollisionPoint CrosswalkModule::createCollisionPoint( return collision_point; } -void CrosswalkModule::applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects, +void CrosswalkModule::applySlowDown( + PathWithLaneId & output, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk, const float safety_slow_down_speed) { - if (path_intersects.empty()) { - return; - } - const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_path = output; std::optional slowdown_pose{std::nullopt}; @@ -851,7 +797,8 @@ void CrosswalkModule::applySafetySlowDownSpeed( const double safety_slow_margin = planner_data_->vehicle_info_.max_longitudinal_offset_m + safety_slow_down_distance; const double safety_slow_point_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()) - safety_slow_margin; + calcSignedArcLength(ego_path.points, ego_pos, first_path_point_on_crosswalk) - + safety_slow_margin; const auto & p_safety_slow = calcLongitudinalOffsetPoint(ego_path.points, ego_pos, safety_slow_point_range); @@ -868,7 +815,7 @@ void CrosswalkModule::applySafetySlowDownSpeed( } else { // the range until to the point where ego will start accelerate const double safety_slow_end_point_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); + calcSignedArcLength(ego_path.points, ego_pos, last_path_point_on_crosswalk); if (0.0 < safety_slow_end_point_range) { // insert constant ego speed until the end of the crosswalk @@ -885,6 +832,68 @@ void CrosswalkModule::applySafetySlowDownSpeed( VelocityFactor::APPROACHING); } +void CrosswalkModule::applySlowDownByLanelet2Map( + PathWithLaneId & output, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk) +{ + if (!crosswalk_.hasAttribute("safety_slow_down_speed")) { + return; + } + applySlowDown( + output, first_path_point_on_crosswalk, last_path_point_on_crosswalk, + static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); +} + +void CrosswalkModule::applySlowDownByOcclusion( + PathWithLaneId & output, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk) +{ + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const auto objects_ptr = planner_data_->predicted_objects; + + const auto now = clock_->now(); + const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) { + return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer); + }; + const auto crosswalk_has_traffic_light = + !crosswalk_.regulatoryElementsAs().empty(); + const auto is_crosswalk_ignored = + (planner_param_.occlusion_ignore_with_traffic_light && crosswalk_has_traffic_light) || + crosswalk_.hasAttribute("skip_occluded_slowdown"); + if (planner_param_.occlusion_enable && !is_crosswalk_ignored) { + const auto dist_ego_to_crosswalk = + calcSignedArcLength(output.points, ego_pos, first_path_point_on_crosswalk); + const auto detection_range = + planner_data_->vehicle_info_.max_lateral_offset_m + + calculate_detection_range( + planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk, + planner_data_->current_velocity->twist.linear.x); + const auto is_ego_on_the_crosswalk = + dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m; + if (!is_ego_on_the_crosswalk) { + if (is_crosswalk_occluded( + crosswalk_, *planner_data_->occupancy_grid, first_path_point_on_crosswalk, + detection_range, objects_ptr->objects, planner_param_)) { + if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = now; + if (cmp_with_time_buffer(current_initial_occlusion_time_, std::greater_equal{})) + most_recent_occlusion_time_ = now; + } else if (!cmp_with_time_buffer(most_recent_occlusion_time_, std::greater{})) { + current_initial_occlusion_time_.reset(); + } + + if (cmp_with_time_buffer(most_recent_occlusion_time_, std::less_equal{})) { + const auto target_velocity = calcTargetVelocity(first_path_point_on_crosswalk, output); + applySlowDown( + output, first_path_point_on_crosswalk, last_path_point_on_crosswalk, + std::max(target_velocity, planner_param_.occlusion_slow_down_velocity)); + debug_data_.virtual_wall_suffix = " (occluded)"; + } else { + most_recent_occlusion_time_.reset(); + } + } + } +} + Polygon2d CrosswalkModule::getAttentionArea( const PathWithLaneId & sparse_resample_path, const std::pair & crosswalk_attention_range) const @@ -927,12 +936,13 @@ Polygon2d CrosswalkModule::getAttentionArea( std::optional CrosswalkModule::checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, - const std::vector & path_intersects, + const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk, const std::optional & stop_pose) { const auto & p = planner_param_; - if (path_intersects.size() < 2 || !stop_pose) { + if (!stop_pose) { return {}; } @@ -964,10 +974,10 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( // check if STOP is required const double crosswalk_front_to_obj_rear = - calcSignedArcLength(ego_path.points, path_intersects.front(), obj_pose.position) - + calcSignedArcLength(ego_path.points, first_path_point_on_crosswalk, obj_pose.position) - object.shape.dimensions.x / 2.0; const double crosswalk_back_to_obj_rear = - calcSignedArcLength(ego_path.points, path_intersects.back(), obj_pose.position) - + calcSignedArcLength(ego_path.points, last_path_point_on_crosswalk, obj_pose.position) - object.shape.dimensions.x / 2.0; const double required_space_length = planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index b7dc2fea16794..ebb9d715ccd6a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -331,23 +331,33 @@ class CrosswalkModule : public SceneModuleInterface private: // main functions - void applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects, - const float speed); + void applySlowDown( + PathWithLaneId & output, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk, + const float safety_slow_down_speed); - std::optional> getStopPointWithMargin( + void applySlowDownByLanelet2Map( + PathWithLaneId & output, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk); + + void applySlowDownByOcclusion( + PathWithLaneId & output, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk); + + std::optional getDefaultStopPose( const PathWithLaneId & ego_path, - const std::vector & path_intersects) const; + const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const; std::optional checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, - const std::optional> & p_stop_line, - const std::vector & path_intersects, + const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk, const std::optional & default_stop_pose); std::optional checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, - const std::vector & path_intersects, + const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk, const std::optional & stop_pose); std::optional findEgoPassageDirectionAlongPath( @@ -378,7 +388,8 @@ class CrosswalkModule : public SceneModuleInterface // minor functions std::pair getAttentionRange( const PathWithLaneId & ego_path, - const std::vector & path_intersects); + const geometry_msgs::msg::Point & first_path_point_on_crosswalk, + const geometry_msgs::msg::Point & last_path_point_on_crosswalk); void insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, @@ -401,10 +412,6 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & sparse_resample_path, const std::pair & crosswalk_attention_range) const; - bool isStuckVehicle( - const PathWithLaneId & ego_path, const std::vector & objects, - const std::vector & path_intersects) const; - void updateObjectState( const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp index 6389637ca1526..755a32b08e024 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/util.cpp @@ -111,14 +111,19 @@ bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ return !lanelet::utils::query::crosswalks(all_lanelets).empty(); } -std::vector getPolygonIntersects( +/** + * @brief Calculate path end (= first and last) points on the crosswalk + * + * @return first and last path points on the crosswalk + */ +std::optional> +getPathEndPointsOnCrosswalk( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, - const geometry_msgs::msg::Point & ego_pos, - const size_t max_num = std::numeric_limits::max()) + const geometry_msgs::msg::Point & ego_pos) { std::vector intersects{}; - bool found_max_num = false; + bool has_collision_twice = false; for (size_t i = 0; i < ego_path.points.size() - 1; ++i) { const auto & p_back = ego_path.points.at(i).point.pose.position; const auto & p_front = ego_path.points.at(i + 1).point.pose.position; @@ -129,13 +134,13 @@ std::vector getPolygonIntersects( for (const auto & p : tmp_intersects) { intersects.push_back(p); - if (intersects.size() == max_num) { - found_max_num = true; + if (intersects.size() == 2) { + has_collision_twice = true; break; } } - if (found_max_num) { + if (has_collision_twice) { break; } } @@ -152,22 +157,24 @@ std::vector getPolygonIntersects( std::sort(intersects.begin(), intersects.end(), compare); - // convert autoware::universe_utils::Point2d to geometry::msg::Point - std::vector geometry_points; - for (const auto & p : intersects) { - geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + if (intersects.empty()) { + return std::nullopt; } - return geometry_points; + + const auto & front_intersects = intersects.front(); + const auto & back_intersects = intersects.back(); + return std::make_pair( + createPoint(front_intersects.x(), front_intersects.y(), ego_pos.z), + createPoint(back_intersects.x(), back_intersects.y(), ego_pos.z)); } std::vector getLinestringIntersects( const PathWithLaneId & ego_path, const lanelet::BasicLineString2d & linestring, - const geometry_msgs::msg::Point & ego_pos, - const size_t max_num = std::numeric_limits::max()) + const geometry_msgs::msg::Point & ego_pos) { std::vector intersects{}; - bool found_max_num = false; + bool has_collision_twice = false; for (size_t i = 0; i < ego_path.points.size() - 1; ++i) { const auto & p_back = ego_path.points.at(i).point.pose.position; const auto & p_front = ego_path.points.at(i + 1).point.pose.position; @@ -178,13 +185,13 @@ std::vector getLinestringIntersects( for (const auto & p : tmp_intersects) { intersects.push_back(p); - if (intersects.size() == max_num) { - found_max_num = true; + if (intersects.size() == 2) { + has_collision_twice = true; break; } } - if (found_max_num) { + if (has_collision_twice) { break; } } 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_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/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 5a64daa0f95fc..765732969951d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -54,14 +54,14 @@ WalkwayModule::WalkwayModule( } } -std::optional> WalkwayModule::getStopLine( +std::pair WalkwayModule::getStopLine( const PathWithLaneId & ego_path, bool & exist_stopline_in_map, - const std::vector & path_intersects) const + const geometry_msgs::msg::Point & first_path_point_on_walkway) const { const auto & ego_pos = planner_data_->current_odometry->pose.position; for (const auto & stop_line : stop_lines_) { - const auto p_stop_lines = getLinestringIntersects( - ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); + const auto p_stop_lines = + getLinestringIntersects(ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos); if (p_stop_lines.empty()) { continue; } @@ -73,18 +73,12 @@ std::optional> WalkwayModule::getSt return std::make_pair(dist_ego_to_stop, p_stop_lines.front()); } - { - exist_stopline_in_map = false; + exist_stopline_in_map = false; - if (!path_intersects.empty()) { - const auto p_stop_line = path_intersects.front(); - const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line) - - planner_param_.stop_distance_from_crosswalk; - return std::make_pair(dist_ego_to_stop, p_stop_line); - } - } - - return {}; + const auto p_stop_line = first_path_point_on_walkway; + const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line) - + planner_param_.stop_distance_from_crosswalk; + return std::make_pair(dist_ego_to_stop, p_stop_line); } bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -97,21 +91,19 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ const auto input = *path; const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto path_intersects = - getPolygonIntersects(input, walkway_.polygon2d().basicPolygon(), ego_pos, 2); - - if (path_intersects.empty()) { + const auto path_end_points_on_walkway = + getPathEndPointsOnCrosswalk(input, walkway_.polygon2d().basicPolygon(), ego_pos); + if (!path_end_points_on_walkway) { return false; } + const auto & first_path_point_on_walkway = path_end_points_on_walkway->first; + if (state_ == State::APPROACH) { bool exist_stopline_in_map; - const auto p_stop_line = getStopLine(input, exist_stopline_in_map, path_intersects); - if (!p_stop_line) { - return false; - } + const auto p_stop_line = getStopLine(input, exist_stopline_in_map, first_path_point_on_walkway); - const auto & p_stop = p_stop_line->second; + const auto & p_stop = p_stop_line.second; const auto stop_distance_from_crosswalk = exist_stopline_in_map ? 0.0 : planner_param_.stop_distance_from_crosswalk; const auto margin = stop_distance_from_crosswalk + base_link2front; @@ -129,7 +121,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ /* get stop point and stop factor */ StopFactor stop_factor; stop_factor.stop_pose = stop_pose.value(); - stop_factor.stop_factor_points.push_back(path_intersects.front()); + stop_factor.stop_factor_points.push_back(path_end_points_on_walkway->first); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose.value(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index 0099de46e1598..a400f57451d2e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -54,9 +54,9 @@ class WalkwayModule : public SceneModuleInterface private: const int64_t module_id_; - [[nodiscard]] std::optional> getStopLine( + [[nodiscard]] std::pair getStopLine( const PathWithLaneId & ego_path, bool & exist_stopline_in_map, - const std::vector & path_intersects) const; + const geometry_msgs::msg::Point & first_path_point_on_walkway) const; enum class State { APPROACH, STOP, SURPASSED }; 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_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/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 c550c9dfb4933..d13f8a12b2327 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 @@ -504,17 +504,17 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZIRCCloud( output_modifier.reserve(input_ptr->width); bool has_valid_intensity = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const 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) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const 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) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; }); diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index d1f02f5f0bbf1..3255df4b8613c 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -338,17 +338,17 @@ void PointCloudConcatenationComponent::convertToXYZIRCCloud( output_modifier.reserve(input_ptr->width); bool has_valid_intensity = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const 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) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const 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) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; }); 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 9aba44be1065c..11b1268b5ac16 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -441,17 +441,17 @@ void PointCloudDataSynchronizerComponent::convertToXYZIRCCloud( output_modifier.reserve(input_ptr->width); bool has_valid_intensity = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const 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) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const 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) { + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; }); diff --git a/simulator/autoware_carla_interface/CMakeLists.txt b/simulator/autoware_carla_interface/CMakeLists.txt new file mode 100644 index 0000000000000..d643b9dad45fe --- /dev/null +++ b/simulator/autoware_carla_interface/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_carla_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package( + catkin REQUIRED COMPONENTS std_msgs) + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +ament_export_dependencies(rclpy) + +find_package(ros_environment REQUIRED) +set(ROS_VERSION $ENV{ROS_VERSION}) + + +# Install launch files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) + + +ament_auto_package( + launch + resource + config + src +) +ament_package() diff --git a/simulator/autoware_carla_interface/README.md b/simulator/autoware_carla_interface/README.md new file mode 100644 index 0000000000000..a44cb4708b5c3 --- /dev/null +++ b/simulator/autoware_carla_interface/README.md @@ -0,0 +1,161 @@ +# autoware_carla_interface + +## ROS 2/Autoware.universe bridge for CARLA simulator + +Thanks to for ROS 2 Humble support for CARLA Communication. +This ros package enables communication between Autoware and CARLA for autonomous driving simulation. + +## Supported Environment + +| ubuntu | ros | carla | autoware | +| :----: | :----: | :----: | :------: | +| 22.04 | humble | 0.9.15 | Main | + +## Setup + +### Install + +- [CARLA Installation](https://carla.readthedocs.io/en/latest/start_quickstart/) +- [Carla Lanelet2 Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/) +- [Python Package for CARLA 0.9.15 ROS 2 Humble communication](https://github.com/gezp/carla_ros/releases/tag/carla-0.9.15-ubuntu-22.04) + + - Install the wheel using pip. + - OR add the egg file to the `PYTHONPATH`. + +1. Download maps (y-axis inverted version) to arbitrary location +2. Change names and create the map folder (example: Town01) inside `autoware_map`. (`point_cloud/Town01.pcd` -> `autoware_map/Town01/pointcloud_map.pcd`, `vector_maps/lanelet2/Town01.osm`-> `autoware_map/Town01/lanelet2_map.osm`) +3. Create `map_projector_info.yaml` on the folder and add `projector_type: local` on the first line. + +### Build + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` + +### Run + +1. Run carla, change map, spawn object if you need + + + ```bash + cd CARLA + ./CarlaUE4.sh -prefernvidia -quality-level=Low -RenderOffScreen + ``` + +2. Run ros nodes + + ```bash + ros2 launch autoware_launch e2e_simulator.launch.xml map_path:=$HOME/autoware_map/Town01 vehicle_model:=sample_vehicle sensor_model:=awsim_sensor_kit simulator_type:=carla carla_map:=Town01 + ``` + +3. Set initial pose (Init by GNSS) +4. Set goal position +5. Wait for planning +6. Engage + +## Inner-workings / Algorithms + +The `InitializeInterface` class is key to setting up both the CARLA world and the ego vehicle. It fetches configuration parameters through the `autoware_carla_interface.launch.xml`. + +The main simulation loop runs within the `carla_ros2_interface` class. This loop ticks simulation time inside the CARLA simulator at `fixed_delta_seconds` time, where data is received and published as ROS 2 messages at frequencies defined in `self.sensor_frequencies`. + +Ego vehicle commands from Autoware are processed through the `autoware_raw_vehicle_cmd_converter`, which calibrates these commands for CARLA. The calibrated commands are then fed directly into CARLA control via `CarlaDataProvider`. + +### Configurable Parameters for World Loading + +All the key parameters can be configured in `autoware_carla_interface.launch.xml`. + +| Name | Type | Default Value | Description | +| ------------------------- | ------ | --------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `host` | string | "localhost" | Hostname for the CARLA server | +| `port` | int | "2000" | Port number for the CARLA server | +| `timeout` | int | 20 | Timeout for the CARLA client | +| `ego_vehicle_role_name` | string | "ego_vehicle" | Role name for the ego vehicle | +| `vehicle_type` | string | "vehicle.toyota.prius" | Blueprint ID of the vehicle to spawn. The Blueprint ID of vehicles can be found in [CARLA Blueprint ID](https://carla.readthedocs.io/en/latest/catalogue_vehicles/) | +| `spawn_point` | string | None | Coordinates for spawning the ego vehicle (None is random). Format = [x, y, z, roll, pitch, yaw] | +| `carla_map` | string | "Town01" | Name of the map to load in CARLA | +| `sync_mode` | bool | True | Boolean flag to set synchronous mode in CARLA | +| `fixed_delta_seconds` | double | 0.05 | Time step for the simulation (related to client FPS) | +| `objects_definition_file` | string | "$(find-pkg-share autoware_carla_interface)/objects.json" | Sensor parameters file that are used for spawning sensor in CARLA | +| `use_traffic_manager` | bool | True | Boolean flag to set traffic manager in CARLA | +| `max_real_delta_seconds` | double | 0.05 | Parameter to limit the simulation speed below `fixed_delta_seconds` | +| `config_file` | string | "$(find-pkg-share autoware_carla_interface)/raw_vehicle_cmd_converter.param.yaml" | Control mapping file to be used in `autoware_raw_vehicle_cmd_converter`. Current control are calibrated based on `vehicle.toyota.prius` Blueprints ID in CARLA. Changing the vehicle type may need a recalibration. | + +### Configurable Parameters for Sensors + +Below parameters can be configured in `carla_ros.py`. + +| Name | Type | Default Value | Description | +| ------------------------- | ---- | -------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `self.sensor_frequencies` | dict | {"top": 11, "left": 11, "right": 11, "camera": 11, "imu": 50, "status": 50, "pose": 2} | (line 67) Calculates the time interval since the last publication and checks if this interval meets the minimum required to not exceed the desired frequency. It will only affect ROS publishing frequency not CARLA sensor tick. | + +- CARLA sensor parameters can be configured in `config/objects.json`. + - For more details regarding the parameters that can be modified in CARLA are explained in [Carla Ref Sensor](https://carla.readthedocs.io/en/latest/ref_sensors/). + +### World Loading + +The `carla_ros.py` sets up the CARLA world: + +1. **Client Connection**: + + ```python + client = carla.Client(self.local_host, self.port) + client.set_timeout(self.timeout) + ``` + +2. **Load the Map**: + + Map loaded in CARLA world with map according to `carla_map` parameter. + + ```python + client.load_world(self.map_name) + self.world = client.get_world() + ``` + +3. **Spawn Ego Vehicle**: + + Vehicle are spawn according to `vehicle_type`, `spawn_point`, and `agent_role_name` parameter. + + ```python + spawn_point = carla.Transform() + point_items = self.spawn_point.split(",") + if len(point_items) == 6: + spawn_point.location.x = float(point_items[0]) + spawn_point.location.y = float(point_items[1]) + spawn_point.location.z = float(point_items[2]) + 2 + spawn_point.rotation.roll = float(point_items[3]) + spawn_point.rotation.pitch = float(point_items[4]) + spawn_point.rotation.yaw = float(point_items[5]) + CarlaDataProvider.request_new_actor(self.vehicle_type, spawn_point, self.agent_role_name) + ``` + +## Traffic Light Recognition + +The maps provided by the Carla Simulator ([Carla Lanelet2 Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/)) currently lack proper traffic light components for Autoware and have different latitude and longitude coordinates compared to the pointcloud map. To enable traffic light recognition, follow the steps below to modify the maps. + +- Options to Modify the Map + + - A. Create a New Map from Scratch + - Use the [Tier4 Vector Map Builder](https://tools.tier4.jp/feature/vector_map_builder_ll2/) to create a new map. + + - B. Modify the Existing Carla Lanelet2 Maps + - Adjust the longitude and latitude of the [Carla Lanelet2 Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/) to align with the PCD (origin). + - Use this [tool](https://github.com/mraditya01/offset_lanelet2/tree/main) to modify the coordinates. + - Snap Lanelet with PCD and add the traffic lights using the [Tier4 Vector Map Builder](https://tools.tier4.jp/feature/vector_map_builder_ll2/). + +- When using the Tier4 Vector Map Builder, you must convert the PCD format from `binary_compressed` to `ascii`. You can use `pcl_tools` for this conversion. +- For reference, an example of Town01 with added traffic lights at one intersection can be downloaded [here](https://drive.google.com/drive/folders/1QFU0p3C8NW71sT5wwdnCKXoZFQJzXfTG?usp=sharing). + +## Tips + +- Misalignment might occurs during initialization, pressing `init by gnss` button should fix it. +- Changing the `fixed_delta_seconds` can increase the simulation tick (default 0.05 s), some sensors params in `objects.json` need to be adjusted when it is changed (example: LIDAR rotation frequency have to match the FPS). + +## Known Issues and Future Works + +- Testing on procedural map (Adv Digital Twin). + - Currently unable to test it due to failing in the creation of the Adv digital twin map. +- Automatic sensor configuration of the CARLA sensors from the Autoware sensor kit. + - Sensor currently not automatically configured to have the same location as the Autoware Sensor kit. The current work around is to create a new frame of each sensors with (0, 0, 0, 0, 0, 0) coordinate relative to base_link and attach each sensor on the new frame (`autoware_carla_interface.launch.xml` Line 28). This work around is very limited and restrictive, as when the sensor_kit is changed the sensor location will be wrongly attached. +- Traffic light recognition. + - Currently the HDmap of CARLA did not have information regarding the traffic light which is necessary for Autoware to conduct traffic light recognition. diff --git a/simulator/autoware_carla_interface/calibration_maps/accel_map.csv b/simulator/autoware_carla_interface/calibration_maps/accel_map.csv new file mode 100644 index 0000000000000..18718c31df87e --- /dev/null +++ b/simulator/autoware_carla_interface/calibration_maps/accel_map.csv @@ -0,0 +1,7 @@ +default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89 +0,0.090,-0.204,-0.490,-0.490,-0.491,-0.492,-0.493,-0.494,-0.495,-0.496,-0.500 +0.100,0.167,0.166,-0.093,-0.243,-0.244,-0.245,-0.246,-0.247,-0.248,-0.249,-0.280 +0.200,0.941,0.464,0.186,0.004,-0.100,-0.101,-0.102,-0.103,-0.104,-0.105,-0.106 +0.300,1.747,1.332,0.779,0.778,0.777,0.776,0.775,0.774,0.720,0.640,0.580 +0.400,2.650,2.480,2.300,2.130,1.950,1.750,1.580,1.450,1.320,1.200,1.100 +0.500,3.300,3.250,3.120,2.920,2.680,2.350,2.170,1.980,1.880,1.730,1.610 diff --git a/simulator/autoware_carla_interface/calibration_maps/brake_map.csv b/simulator/autoware_carla_interface/calibration_maps/brake_map.csv new file mode 100644 index 0000000000000..62b18b45bd415 --- /dev/null +++ b/simulator/autoware_carla_interface/calibration_maps/brake_map.csv @@ -0,0 +1,10 @@ +default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89 +0,0.090,-0.204,-0.490,-0.490,-0.491,-0.492,-0.493,-0.494,-0.495,-0.496,-0.500 +0.100,0.089,-0.226,-0.535,-0.536,-0.537,-0.538,-0.539,-0.540,-0.541,-0.542,-0.543 +0.200,-0.380,-0.414,-0.746,-0.800,-0.820,-0.850,-0.870,-0.890,-0.910,-0.940,-0.960 +0.300,-1.000,-1.040,-1.480,-1.550,-1.570,-1.590,-1.610,-1.630,-1.631,-1.632,-1.633 +0.400,-1.480,-1.500,-1.850,-2.050,-2.100,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 +0.500,-1.490,-1.510,-1.860,-2.060,-2.110,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 +0.600,-1.500,-1.520,-1.870,-2.070,-2.120,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 +0.700,-1.510,-1.530,-1.880,-2.080,-2.130,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 +0.800,-2.180,-2.200,-2.700,-2.800,-2.900,-2.950,-2.951,-2.952,-2.953,-2.954,-2.955 diff --git a/simulator/autoware_carla_interface/calibration_maps/steer_map.csv b/simulator/autoware_carla_interface/calibration_maps/steer_map.csv new file mode 100644 index 0000000000000..077efb9f9e200 --- /dev/null +++ b/simulator/autoware_carla_interface/calibration_maps/steer_map.csv @@ -0,0 +1,4 @@ +default,-10,0,10 +-1,-0.9,-0.9,-0.9 +0,0,0,0 +1,0.9,0.9,0.9 diff --git a/simulator/autoware_carla_interface/config/objects.json b/simulator/autoware_carla_interface/config/objects.json new file mode 100644 index 0000000000000..7103118937548 --- /dev/null +++ b/simulator/autoware_carla_interface/config/objects.json @@ -0,0 +1,62 @@ +{ + "sensors": [ + { + "type": "sensor.camera.rgb", + "id": "rgb_front", + "spawn_point": { + "x": 0.7, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + }, + "image_size_x": 1920, + "image_size_y": 1080, + "fov": 90.0 + }, + { + "type": "sensor.lidar.ray_cast", + "id": "top", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 3.1, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + }, + "range": 100, + "channels": 64, + "points_per_second": 300000, + "upper_fov": 10.0, + "lower_fov": -30.0, + "rotation_frequency": 20, + "noise_stddev": 0.0 + }, + { + "type": "sensor.other.gnss", + "id": "gnss", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + } + }, + { + "type": "sensor.other.imu", + "id": "imu", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + } + } + ] +} diff --git a/simulator/autoware_carla_interface/config/raw_vehicle_cmd_converter.param.yaml b/simulator/autoware_carla_interface/config/raw_vehicle_cmd_converter.param.yaml new file mode 100644 index 0000000000000..cb604bc686075 --- /dev/null +++ b/simulator/autoware_carla_interface/config/raw_vehicle_cmd_converter.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + csv_path_accel_map: $(find-pkg-share autoware_carla_interface)/accel_map.csv + csv_path_brake_map: $(find-pkg-share autoware_carla_interface)/brake_map.csv + csv_path_steer_map: $(find-pkg-share autoware_carla_interface)/steer_map.csv + convert_accel_cmd: true + convert_brake_cmd: true + convert_steer_cmd: false + use_steer_ff: true + use_steer_fb: true + is_debugging: false + max_throttle: 0.4 + max_brake: 0.8 + max_steer: 1.0 + min_steer: -1.0 + steer_pid: + kp: 150.0 + ki: 15.0 + kd: 0.0 + max: 8.0 + min: -8.0 + max_p: 8.0 + min_p: -8.0 + max_i: 8.0 + min_i: -8.0 + max_d: 0.0 + min_d: 0.0 + invalid_integration_decay: 0.97 diff --git a/simulator/autoware_carla_interface/launch/autoware_carla_interface.launch.xml b/simulator/autoware_carla_interface/launch/autoware_carla_interface.launch.xml new file mode 100644 index 0000000000000..bdef2563777fc --- /dev/null +++ b/simulator/autoware_carla_interface/launch/autoware_carla_interface.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/autoware_carla_interface/package.xml b/simulator/autoware_carla_interface/package.xml new file mode 100644 index 0000000000000..136138a0101b0 --- /dev/null +++ b/simulator/autoware_carla_interface/package.xml @@ -0,0 +1,29 @@ + + + autoware_carla_interface + 0.0.0 + The carla autoware bridge package + Muhammad Raditya GIOVANNI + Maxime CLEMENT + Apache License 2.0 + + std_msgs + autoware_perception_msgs + autoware_vehicle_msgs + geometry_msgs + rclpy + sensor_msgs + sensor_msgs_py + tf2 + tf2_ros + tier4_vehicle_msgs + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_python + + diff --git a/simulator/autoware_carla_interface/resource/carla_autoware b/simulator/autoware_carla_interface/resource/carla_autoware new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/simulator/autoware_carla_interface/setup.cfg b/simulator/autoware_carla_interface/setup.cfg new file mode 100644 index 0000000000000..c749fdbef61a2 --- /dev/null +++ b/simulator/autoware_carla_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/autoware_carla_interface +[install] +install_scripts=$base/lib/autoware_carla_interface diff --git a/simulator/autoware_carla_interface/setup.py b/simulator/autoware_carla_interface/setup.py new file mode 100644 index 0000000000000..f4bf37db01fb9 --- /dev/null +++ b/simulator/autoware_carla_interface/setup.py @@ -0,0 +1,33 @@ +from glob import glob +import os + +from setuptools import setup + +ROS_VERSION = int(os.environ["ROS_VERSION"]) + +package_name = "autoware_carla_interface" + +setup( + name=package_name, + version="0.0.0", + packages=[package_name], + data_files=[ + ("share/" + package_name, glob("config/*")), + ("share/" + package_name, glob("calibration_maps/*.csv")), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name), glob("launch/autoware_carla_interface.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Muhammad Raditya GIOVANNI, Maxime CLEMENT", + maintainer_email="mradityagio@gmail.com, maxime.clement@tier4.jp", + description="CARLA ROS 2 bridge for AUTOWARE", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "autoware_carla_interface = autoware_carla_interface.carla_autoware:main" + ], + }, + package_dir={"": "src"}, +) diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py new file mode 100644 index 0000000000000..b9c72d6c137d9 --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py @@ -0,0 +1,195 @@ +# 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.sr/bin/env python + +import random +import signal +import time + +import carla + +from .carla_ros import carla_ros2_interface +from .modules.carla_data_provider import CarlaDataProvider +from .modules.carla_data_provider import GameTime +from .modules.carla_wrapper import SensorReceivedNoData +from .modules.carla_wrapper import SensorWrapper + + +class SensorLoop(object): + def __init__(self): + self.start_game_time = None + self.start_system_time = None + self.sensor = None + self.ego_actor = None + self.running = False + self.timestamp_last_run = 0.0 + self.timeout = 20.0 + + def _stop_loop(self): + self.running = False + + def _tick_sensor(self, timestamp): + if self.timestamp_last_run < timestamp.elapsed_seconds and self.running: + self.timestamp_last_run = timestamp.elapsed_seconds + GameTime.on_carla_tick(timestamp) + CarlaDataProvider.on_carla_tick() + try: + ego_action = self.sensor() + except SensorReceivedNoData as e: + raise RuntimeError(e) + self.ego_actor.apply_control(ego_action) + if self.running: + CarlaDataProvider.get_world().tick() + + +class InitializeInterface(object): + def __init__(self): + self.interface = carla_ros2_interface() + self.param_ = self.interface.get_param() + self.world = None + self.sensor_wrapper = None + self.ego_actor = None + self.prev_tick_wall_time = 0.0 + + # Parameter for Initializing Carla World + self.local_host = self.param_["host"] + self.port = self.param_["port"] + self.timeout = self.param_["timeout"] + self.sync_mode = self.param_["sync_mode"] + self.fixed_delta_seconds = self.param_["fixed_delta_seconds"] + self.carla_map = self.param_["carla_map"] + self.agent_role_name = self.param_["ego_vehicle_role_name"] + self.vehicle_type = self.param_["vehicle_type"] + self.spawn_point = self.param_["spawn_point"] + self.use_traffic_manager = self.param_["use_traffic_manager"] + self.max_real_delta_seconds = self.param_["max_real_delta_seconds"] + + def load_world(self): + client = carla.Client(self.local_host, self.port) + client.set_timeout(self.timeout) + client.load_world(self.carla_map) + self.world = client.get_world() + settings = self.world.get_settings() + settings.fixed_delta_seconds = self.fixed_delta_seconds + settings.synchronous_mode = self.sync_mode + self.world.apply_settings(settings) + CarlaDataProvider.set_world(self.world) + CarlaDataProvider.set_client(client) + spawn_point = carla.Transform() + point_items = self.spawn_point.split(",") + randomize = False + if len(point_items) == 6: + spawn_point.location.x = float(point_items[0]) + spawn_point.location.y = float(point_items[1]) + spawn_point.location.z = ( + float(point_items[2]) + 2 + ) # +2 is used so the car did not stuck on the road when spawned. + spawn_point.rotation.roll = float(point_items[3]) + spawn_point.rotation.pitch = float(point_items[4]) + spawn_point.rotation.yaw = float(point_items[5]) + else: + randomize = True + self.ego_actor = CarlaDataProvider.request_new_actor( + self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize + ) + self.interface.ego_actor = self.ego_actor # TODO improve design + self.interface.physics_control = self.ego_actor.get_physics_control() + + self.sensor_wrapper = SensorWrapper(self.interface) + self.sensor_wrapper.setup_sensors(self.ego_actor, False) + ########################################################################################################################################################## + # TRAFFIC MANAGER + ########################################################################################################################################################## + # cspell:ignore trafficmanager + if self.use_traffic_manager: + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + traffic_manager.set_random_device_seed(0) + random.seed(0) + spawn_points_tm = self.world.get_map().get_spawn_points() + for i, spawn_point in enumerate(spawn_points_tm): + self.world.debug.draw_string(spawn_point.location, str(i), life_time=10) + models = [ + "dodge", + "audi", + "model3", + "mini", + "mustang", + "lincoln", + "prius", + "nissan", + "crown", + "impala", + ] + blueprints = [] + for vehicle in self.world.get_blueprint_library().filter("*vehicle*"): + if any(model in vehicle.id for model in models): + blueprints.append(vehicle) + max_vehicles = 30 + max_vehicles = min([max_vehicles, len(spawn_points_tm)]) + vehicles = [] + for i, spawn_point in enumerate(random.sample(spawn_points_tm, max_vehicles)): + temp = self.world.try_spawn_actor(random.choice(blueprints), spawn_point) + if temp is not None: + vehicles.append(temp) + + for vehicle in vehicles: + vehicle.set_autopilot(True) + + def run_bridge(self): + self.bridge_loop = SensorLoop() + self.bridge_loop.sensor = self.sensor_wrapper + self.bridge_loop.ego_actor = self.ego_actor + self.bridge_loop.start_system_time = time.time() + self.bridge_loop.start_game_time = GameTime.get_time() + self.bridge_loop.running = True + while self.bridge_loop.running: + timestamp = None + world = CarlaDataProvider.get_world() + if world: + snapshot = world.get_snapshot() + if snapshot: + timestamp = snapshot.timestamp + if timestamp: + delta_step = time.time() - self.prev_tick_wall_time + if delta_step <= self.max_real_delta_seconds: + # Add a wait to match the max_real_delta_seconds + time.sleep(self.max_real_delta_seconds - delta_step) + self.prev_tick_wall_time = time.time() + self.bridge_loop._tick_sensor(timestamp) + + def _stop_loop(self, sign, frame): + self.bridge_loop._stop_loop() + + def _cleanup(self): + self.sensor_wrapper.cleanup() + CarlaDataProvider.cleanup() + if self.ego_actor: + self.ego_actor.destroy() + self.ego_actor = None + + if self.interface: + self.interface.shutdown() + self.interface = None + + +def main(): + carla_bridge = InitializeInterface() + carla_bridge.load_world() + signal.signal(signal.SIGINT, carla_bridge._stop_loop) + carla_bridge.run_bridge() + carla_bridge._cleanup() + + +if __name__ == "__main__": + main() diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py new file mode 100644 index 0000000000000..a4f8dee7af1a0 --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py @@ -0,0 +1,486 @@ +# 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.sr/bin/env python + +import json +import math +import threading + +from autoware_vehicle_msgs.msg import ControlModeReport +from autoware_vehicle_msgs.msg import GearReport +from autoware_vehicle_msgs.msg import SteeringReport +from autoware_vehicle_msgs.msg import VelocityReport +from builtin_interfaces.msg import Time +import carla +from cv_bridge import CvBridge +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseWithCovarianceStamped +import numpy +import rclpy +from rosgraph_msgs.msg import Clock +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image +from sensor_msgs.msg import Imu +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from std_msgs.msg import Header +from tier4_vehicle_msgs.msg import ActuationCommandStamped +from tier4_vehicle_msgs.msg import ActuationStatusStamped +from transforms3d.euler import euler2quat + +from .modules.carla_data_provider import GameTime +from .modules.carla_data_provider import datetime +from .modules.carla_utils import carla_location_to_ros_point +from .modules.carla_utils import carla_rotation_to_ros_quaternion +from .modules.carla_utils import create_cloud +from .modules.carla_utils import ros_pose_to_carla_transform +from .modules.carla_wrapper import SensorInterface + + +class carla_ros2_interface(object): + def __init__(self): + self.sensor_interface = SensorInterface() + self.timestamp = None + self.ego_actor = None + self.physics_control = None + self.channels = 0 + self.id_to_sensor_type_map = {} + self.id_to_camera_info_map = {} + self.cv_bridge = CvBridge() + self.first_ = True + self.pub_lidar = {} + self.sensor_frequencies = { + "top": 11, + "left": 11, + "right": 11, + "camera": 11, + "imu": 50, + "status": 50, + "pose": 2, + } + self.publish_prev_times = { + sensor: datetime.datetime.now() for sensor in self.sensor_frequencies + } + + # initialize ros2 node + rclpy.init(args=None) + self.ros2_node = rclpy.create_node("carla_ros2_interface") + self.parameters = { + "host": rclpy.Parameter.Type.STRING, + "port": rclpy.Parameter.Type.INTEGER, + "sync_mode": rclpy.Parameter.Type.BOOL, + "timeout": rclpy.Parameter.Type.INTEGER, + "fixed_delta_seconds": rclpy.Parameter.Type.DOUBLE, + "carla_map": rclpy.Parameter.Type.STRING, + "ego_vehicle_role_name": rclpy.Parameter.Type.STRING, + "spawn_point": rclpy.Parameter.Type.STRING, + "vehicle_type": rclpy.Parameter.Type.STRING, + "objects_definition_file": rclpy.Parameter.Type.STRING, + "use_traffic_manager": rclpy.Parameter.Type.BOOL, + "max_real_delta_seconds": rclpy.Parameter.Type.DOUBLE, + } + self.param_values = {} + for param_name, param_type in self.parameters.items(): + self.ros2_node.declare_parameter(param_name, param_type) + self.param_values[param_name] = self.ros2_node.get_parameter(param_name).value + + # Publish clock + self.clock_publisher = self.ros2_node.create_publisher(Clock, "/clock", 10) + obj_clock = Clock() + obj_clock.clock = Time(sec=0) + self.clock_publisher.publish(obj_clock) + + # Sensor Config (Edit your sensor here) + self.sensors = json.load(open(self.param_values["objects_definition_file"])) + + # Subscribing Autoware Control messages and converting to CARLA control + self.sub_control = self.ros2_node.create_subscription( + ActuationCommandStamped, "/control/command/actuation_cmd", self.control_callback, 1 + ) + + self.sub_vehicle_initialpose = self.ros2_node.create_subscription( + PoseWithCovarianceStamped, "initialpose", self.initialpose_callback, 1 + ) + + self.current_control = carla.VehicleControl() + + # Direct data publishing from CARLA for Autoware + self.pub_pose_with_cov = self.ros2_node.create_publisher( + PoseWithCovarianceStamped, "/sensing/gnss/pose_with_covariance", 1 + ) + self.pub_vel_state = self.ros2_node.create_publisher( + VelocityReport, "/vehicle/status/velocity_status", 1 + ) + self.pub_steering_state = self.ros2_node.create_publisher( + SteeringReport, "/vehicle/status/steering_status", 1 + ) + self.pub_ctrl_mode = self.ros2_node.create_publisher( + ControlModeReport, "/vehicle/status/control_mode", 1 + ) + self.pub_gear_state = self.ros2_node.create_publisher( + GearReport, "/vehicle/status/gear_status", 1 + ) + self.pub_actuation_status = self.ros2_node.create_publisher( + ActuationStatusStamped, "/vehicle/status/actuation_status", 1 + ) + + # Create Publisher for each Physical Sensors + for sensor in self.sensors["sensors"]: + self.id_to_sensor_type_map[sensor["id"]] = sensor["type"] + if sensor["type"] == "sensor.camera.rgb": + self.pub_camera = self.ros2_node.create_publisher( + Image, "/sensing/camera/traffic_light/image_raw", 1 + ) + self.pub_camera_info = self.ros2_node.create_publisher( + CameraInfo, "/sensing/camera/traffic_light/camera_info", 1 + ) + elif sensor["type"] == "sensor.lidar.ray_cast": + if sensor["id"] in self.sensor_frequencies: + self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher( + PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud', 10 + ) + else: + self.ros2_node.get_logger().info( + "Please use Top, Right, or Left as the LIDAR ID" + ) + elif sensor["type"] == "sensor.other.imu": + self.pub_imu = self.ros2_node.create_publisher( + Imu, "/sensing/imu/tamagawa/imu_raw", 1 + ) + else: + self.ros2_node.get_logger().info(f'No Publisher for {sensor["type"]} Sensor') + pass + + self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros2_node,)) + self.spin_thread.start() + + def __call__(self): + input_data = self.sensor_interface.get_data() + timestamp = GameTime.get_time() + control = self.run_step(input_data, timestamp) + return control + + def get_param(self): + return self.param_values + + def checkFrequency(self, sensor): + time_delta = ( + datetime.datetime.now() - self.publish_prev_times[sensor] + ).microseconds / 1000000.0 + if 1.0 / time_delta >= self.sensor_frequencies[sensor]: + return True + return False + + def get_msg_header(self, frame_id): + """Obtain and modify ROS message header.""" + header = Header() + header.frame_id = frame_id + seconds = int(self.timestamp) + nanoseconds = int((self.timestamp - int(self.timestamp)) * 1000000000.0) + header.stamp = Time(sec=seconds, nanosec=nanoseconds) + return header + + def lidar(self, carla_lidar_measurement, id_): + """Transform the received lidar measurement into a ROS point cloud message.""" + if self.checkFrequency(id_): + return + self.publish_prev_times[id_] = datetime.datetime.now() + + header = self.get_msg_header(frame_id="velodyne_top_changed") + fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name="intensity", offset=12, datatype=PointField.UINT8, count=1), + PointField(name="return_type", offset=13, datatype=PointField.UINT8, count=1), + PointField(name="channel", offset=14, datatype=PointField.UINT16, count=1), + ] + + lidar_data = numpy.frombuffer( + carla_lidar_measurement.raw_data, dtype=numpy.float32 + ).reshape(-1, 4) + intensity = lidar_data[:, 3] + intensity = ( + numpy.clip(intensity, 0, 1) * 255 + ) # CARLA lidar intensity values are between 0 and 1 + intensity = intensity.astype(numpy.uint8).reshape(-1, 1) + + return_type = numpy.zeros((lidar_data.shape[0], 1), dtype=numpy.uint8) + channel = numpy.empty((0, 1), dtype=numpy.uint16) + self.channels = self.sensors["sensors"] + + for i in range(self.channels[1]["channels"]): + current_ring_points_count = carla_lidar_measurement.get_point_count(i) + channel = numpy.vstack( + (channel, numpy.full((current_ring_points_count, 1), i, dtype=numpy.uint16)) + ) + + lidar_data = numpy.hstack((lidar_data[:, :3], intensity, return_type, channel)) + lidar_data[:, 1] *= -1 + + dtype = [ + ("x", "f4"), + ("y", "f4"), + ("z", "f4"), + ("intensity", "u1"), + ("return_type", "u1"), + ("channel", "u2"), + ] + + structured_lidar_data = numpy.zeros(lidar_data.shape[0], dtype=dtype) + structured_lidar_data["x"] = lidar_data[:, 0] + structured_lidar_data["y"] = lidar_data[:, 1] + structured_lidar_data["z"] = lidar_data[:, 2] + structured_lidar_data["intensity"] = lidar_data[:, 3].astype(numpy.uint8) + structured_lidar_data["return_type"] = lidar_data[:, 4].astype(numpy.uint8) + structured_lidar_data["channel"] = lidar_data[:, 5].astype(numpy.uint16) + + point_cloud_msg = create_cloud(header, fields, structured_lidar_data) + self.pub_lidar[id_].publish(point_cloud_msg) + + def initialpose_callback(self, data): + """Transform RVIZ initial pose to CARLA.""" + pose = data.pose.pose + pose.position.z += 2.0 + carla_pose_transform = ros_pose_to_carla_transform(pose) + if self.ego_actor is not None: + self.ego_actor.set_transform(carla_pose_transform) + else: + print("Can't find Ego Vehicle") + + def pose(self): + """Transform odometry data to Pose and publish Pose with Covariance message.""" + if self.checkFrequency("pose"): + return + self.publish_prev_times["pose"] = datetime.datetime.now() + + header = self.get_msg_header(frame_id="map") + out_pose_with_cov = PoseWithCovarianceStamped() + pose_carla = Pose() + pose_carla.position = carla_location_to_ros_point(self.ego_actor.get_transform().location) + pose_carla.orientation = carla_rotation_to_ros_quaternion( + self.ego_actor.get_transform().rotation + ) + out_pose_with_cov.header = header + out_pose_with_cov.pose.pose = pose_carla + out_pose_with_cov.pose.covariance = [ + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + self.pub_pose_with_cov.publish(out_pose_with_cov) + + def _build_camera_info(self, camera_actor): + """Build camera info.""" + camera_info = CameraInfo() + camera_info.width = camera_actor.width + camera_info.height = camera_actor.height + camera_info.distortion_model = "plumb_bob" + cx = camera_info.width / 2.0 + cy = camera_info.height / 2.0 + fx = camera_info.width / (2.0 * math.tan(camera_actor.fov * math.pi / 360.0)) + fy = fx + camera_info.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] + camera_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] + camera_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + camera_info.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] + self._camera_info = camera_info + + def camera(self, carla_camera_data): + """Transform the received carla camera data into a ROS image and info message and publish.""" + while self.first_: + self._camera_info_ = self._build_camera_info(carla_camera_data) + self.first_ = False + + if self.checkFrequency("camera"): + return + self.publish_prev_times["camera"] = datetime.datetime.now() + + image_data_array = numpy.ndarray( + shape=(carla_camera_data.height, carla_camera_data.width, 4), + dtype=numpy.uint8, + buffer=carla_camera_data.raw_data, + ) + # cspell:ignore interp bgra + img_msg = self.cv_bridge.cv2_to_imgmsg(image_data_array, encoding="bgra8") + img_msg.header = self.get_msg_header( + frame_id="traffic_light_left_camera/camera_optical_link" + ) + cam_info = self._camera_info + cam_info.header = img_msg.header + self.pub_camera_info.publish(cam_info) + self.pub_camera.publish(img_msg) + + def imu(self, carla_imu_measurement): + """Transform a received imu measurement into a ROS Imu message and publish Imu message.""" + if self.checkFrequency("imu"): + return + self.publish_prev_times["imu"] = datetime.datetime.now() + + imu_msg = Imu() + imu_msg.header = self.get_msg_header(frame_id="tamagawa/imu_link_changed") + imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x + imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y + imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z + + imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x + imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y + imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z + + roll = math.radians(carla_imu_measurement.transform.rotation.roll) + pitch = -math.radians(carla_imu_measurement.transform.rotation.pitch) + yaw = -math.radians(carla_imu_measurement.transform.rotation.yaw) + + quat = euler2quat(roll, pitch, yaw) + imu_msg.orientation.w = quat[0] + imu_msg.orientation.x = quat[1] + imu_msg.orientation.y = quat[2] + imu_msg.orientation.z = quat[3] + + self.pub_imu.publish(imu_msg) + + def control_callback(self, in_cmd): + """Convert and publish CARLA Ego Vehicle Control to AUTOWARE.""" + out_cmd = carla.VehicleControl() + out_cmd.throttle = in_cmd.actuation.accel_cmd + # convert base on steer curve of the vehicle + steer_curve = self.physics_control.steering_curve + current_vel = self.ego_actor.get_velocity() + max_steer_ratio = numpy.interp( + abs(current_vel.x), [v.x for v in steer_curve], [v.y for v in steer_curve] + ) + out_cmd.steer = ( + -in_cmd.actuation.steer_cmd + * max_steer_ratio + * math.radians(self.physics_control.wheels[0].max_steer_angle) + ) + out_cmd.brake = in_cmd.actuation.brake_cmd + self.current_control = out_cmd + + def ego_status(self): + """Publish ego vehicle status.""" + if self.checkFrequency("status"): + return + + self.publish_prev_times["status"] = datetime.datetime.now() + + # convert velocity from cartesian to ego frame + trans_mat = numpy.array(self.ego_actor.get_transform().get_matrix()).reshape(4, 4) + rot_mat = trans_mat[0:3, 0:3] + inv_rot_mat = rot_mat.T + vel_vec = numpy.array( + [ + self.ego_actor.get_velocity().x, + self.ego_actor.get_velocity().y, + self.ego_actor.get_velocity().z, + ] + ).reshape(3, 1) + ego_velocity = (inv_rot_mat @ vel_vec).T[0] + + out_vel_state = VelocityReport() + out_steering_state = SteeringReport() + out_ctrl_mode = ControlModeReport() + out_gear_state = GearReport() + out_actuation_status = ActuationStatusStamped() + + out_vel_state.header = self.get_msg_header(frame_id="base_link") + out_vel_state.longitudinal_velocity = ego_velocity[0] + out_vel_state.lateral_velocity = ego_velocity[1] + out_vel_state.heading_rate = ( + self.ego_actor.get_transform().transform_vector(self.ego_actor.get_angular_velocity()).z + ) + + out_steering_state.stamp = out_vel_state.header.stamp + out_steering_state.steering_tire_angle = -math.radians( + self.ego_actor.get_wheel_steer_angle(carla.VehicleWheelLocation.FL_Wheel) + ) + + out_gear_state.stamp = out_vel_state.header.stamp + out_gear_state.report = GearReport.DRIVE + + out_ctrl_mode.stamp = out_vel_state.header.stamp + out_ctrl_mode.mode = ControlModeReport.AUTONOMOUS + + control = self.ego_actor.get_control() + out_actuation_status.header = self.get_msg_header(frame_id="base_link") + out_actuation_status.status.accel_status = control.throttle + out_actuation_status.status.brake_status = control.brake + out_actuation_status.status.steer_status = -control.steer + + self.pub_actuation_status.publish(out_actuation_status) + self.pub_vel_state.publish(out_vel_state) + self.pub_steering_state.publish(out_steering_state) + self.pub_ctrl_mode.publish(out_ctrl_mode) + self.pub_gear_state.publish(out_gear_state) + + def run_step(self, input_data, timestamp): + self.timestamp = timestamp + seconds = int(self.timestamp) + nanoseconds = int((self.timestamp - int(self.timestamp)) * 1000000000.0) + obj_clock = Clock() + obj_clock.clock = Time(sec=seconds, nanosec=nanoseconds) + self.clock_publisher.publish(obj_clock) + + # publish data of all sensors + for key, data in input_data.items(): + sensor_type = self.id_to_sensor_type_map[key] + if sensor_type == "sensor.camera.rgb": + self.camera(data[1]) + elif sensor_type == "sensor.other.gnss": + self.pose() + elif sensor_type == "sensor.lidar.ray_cast": + self.lidar(data[1], key) + elif sensor_type == "sensor.other.imu": + self.imu(data[1]) + else: + self.ros2_node.get_logger().info("No Publisher for [{key}] Sensor") + + # Publish ego vehicle status + self.ego_status() + return self.current_control + + def shutdown(self): + self.ros2_node.destroy_node() diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py new file mode 100644 index 0000000000000..7d6608585f582 --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py @@ -0,0 +1,867 @@ +# 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.sr/bin/env python + +"""Modified CARLA Data Provider from CARLA scenario runner.""" + +from __future__ import print_function + +import datetime +import math +import re +import threading + +import carla +from numpy import random +from six import iteritems + + +def calculate_velocity(actor): + """Calculate the velocity of a actor.""" + velocity_squared = actor.get_velocity().x ** 2 + velocity_squared += actor.get_velocity().y ** 2 + return math.sqrt(velocity_squared) + + +class CarlaDataProvider(object): # pylint: disable=too-many-public-methods + _actor_velocity_map = {} + _actor_location_map = {} + _actor_transform_map = {} + _traffic_light_map = {} + _carla_actor_pool = {} + _global_osc_parameters = {} + _client = None + _world = None + _map = None + _sync_flag = False + _spawn_points = None + _spawn_index = 0 + _blueprint_library = None + _all_actors = None + _ego_vehicle_route = None + _traffic_manager_port = 8000 + _random_seed = 2000 + _rng = random.RandomState(_random_seed) + _local_planner = None + _runtime_init_flag = False + _lock = threading.Lock() + + @staticmethod + def set_local_planner(plan): + CarlaDataProvider._local_planner = plan + + @staticmethod + def get_local_planner(): + return CarlaDataProvider._local_planner + + @staticmethod + def register_actor(actor, transform=None): + """Add new actor to dictionaries.""" + with CarlaDataProvider._lock: + if actor in CarlaDataProvider._actor_velocity_map: + raise KeyError( + "Vehicle '{}' already registered. Cannot register twice!".format(actor.id) + ) + else: + CarlaDataProvider._actor_velocity_map[actor] = 0.0 + if actor in CarlaDataProvider._actor_location_map: + raise KeyError( + "Vehicle '{}' already registered. Cannot register twice!".format(actor.id) + ) + elif transform: + CarlaDataProvider._actor_location_map[actor] = transform.location + else: + CarlaDataProvider._actor_location_map[actor] = None + + if actor in CarlaDataProvider._actor_transform_map: + raise KeyError( + "Vehicle '{}' already registered. Cannot register twice!".format(actor.id) + ) + else: + CarlaDataProvider._actor_transform_map[actor] = transform + + @staticmethod + def update_osc_global_params(parameters): + """Updates/initializes global osc parameters.""" + CarlaDataProvider._global_osc_parameters.update(parameters) + + @staticmethod + def get_osc_global_param_value(ref): + """Return updated global osc parameter value.""" + return CarlaDataProvider._global_osc_parameters.get(ref.replace("$", "")) + + @staticmethod + def register_actors(actors, transforms=None): + """Add new set of actors to dictionaries.""" + if transforms is None: + transforms = [None] * len(actors) + + for actor, transform in zip(actors, transforms): + CarlaDataProvider.register_actor(actor, transform) + + @staticmethod + def on_carla_tick(): + with CarlaDataProvider._lock: + for actor in CarlaDataProvider._actor_velocity_map: + if actor is not None and actor.is_alive: + CarlaDataProvider._actor_velocity_map[actor] = calculate_velocity(actor) + + for actor in CarlaDataProvider._actor_location_map: + if actor is not None and actor.is_alive: + CarlaDataProvider._actor_location_map[actor] = actor.get_location() + + for actor in CarlaDataProvider._actor_transform_map: + if actor is not None and actor.is_alive: + CarlaDataProvider._actor_transform_map[actor] = actor.get_transform() + + world = CarlaDataProvider._world + if world is None: + print("WARNING: CarlaDataProvider couldn't find the world") + + CarlaDataProvider._all_actors = None + + @staticmethod + def get_velocity(actor): + """Return the absolute velocity for the given actor.""" + for key in CarlaDataProvider._actor_velocity_map: + if key.id == actor.id: + return CarlaDataProvider._actor_velocity_map[key] + + # We are intentionally not throwing here + # This may cause exception loops in py_trees + print("{}.get_velocity: {} not found!".format(__name__, actor)) + return 0.0 + + @staticmethod + def get_location(actor): + """Return the location for the given actor.""" + for key in CarlaDataProvider._actor_location_map: + if key.id == actor.id: + return CarlaDataProvider._actor_location_map[key] + + # We are intentionally not throwing here + # This may cause exception loops in py_trees + print("{}.get_location: {} not found!".format(__name__, actor)) + return None + + @staticmethod + def get_transform(actor): + """Return the transform for the given actor.""" + for key in CarlaDataProvider._actor_transform_map: + if key.id == actor.id: + # The velocity location information is the entire behavior tree updated every tick + # The ego vehicle is created before the behavior tree tick, so exception handling needs to be added + if CarlaDataProvider._actor_transform_map[key] is None: + return actor.get_transform() + return CarlaDataProvider._actor_transform_map[key] + + # We are intentionally not throwing here + # This may cause exception loops in py_trees + print("{}.get_transform: {} not found!".format(__name__, actor)) + return None + + @staticmethod + def get_random_seed(): + """Return the random seed.""" + return CarlaDataProvider._rng + + @staticmethod + def set_client(client): + """Set the CARLA client.""" + CarlaDataProvider._client = client + + @staticmethod + def get_client(): + """Get the CARLA client.""" + return CarlaDataProvider._client + + @staticmethod + def set_world(world): + """Set the world and world settings.""" + CarlaDataProvider._world = world + CarlaDataProvider._sync_flag = world.get_settings().synchronous_mode + CarlaDataProvider._map = world.get_map() + CarlaDataProvider._blueprint_library = world.get_blueprint_library() + CarlaDataProvider.generate_spawn_points() + CarlaDataProvider.prepare_map() + + @staticmethod + def get_world(): + """Return world.""" + return CarlaDataProvider._world + + @staticmethod + def get_map(world=None): + """Get the current map.""" + if CarlaDataProvider._map is None: + if world is None: + if CarlaDataProvider._world is None: + raise ValueError("class member 'world'' not initialized yet") + else: + CarlaDataProvider._map = CarlaDataProvider._world.get_map() + else: + CarlaDataProvider._map = world.get_map() + + return CarlaDataProvider._map + + @staticmethod + def get_all_actors(): + """Return all the world actors.""" + if CarlaDataProvider._all_actors: + return CarlaDataProvider._all_actors + + CarlaDataProvider._all_actors = CarlaDataProvider._world.get_actors() + return CarlaDataProvider._all_actors + + @staticmethod + def set_runtime_init_mode(flag): + """Set the runtime init mode.""" + CarlaDataProvider._runtime_init_flag = flag + + @staticmethod + def is_runtime_init_mode(): + """Return true if runtime init mode is used.""" + return CarlaDataProvider._runtime_init_flag + + @staticmethod + def find_weather_presets(): + """Get weather presets from CARLA.""" + rgx = re.compile(".+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)") + + def format_name(x): + return " ".join(m.group(0) for m in rgx.finditer(x)) + + presets = [x for x in dir(carla.WeatherParameters) if re.match("[A-Z].+", x)] + return [(getattr(carla.WeatherParameters, x), format_name(x)) for x in presets] + + @staticmethod + def prepare_map(): + """Set the current map and loads all traffic lights for this map to_traffic_light_map.""" + if CarlaDataProvider._map is None: + CarlaDataProvider._map = CarlaDataProvider._world.get_map() + + # Parse all traffic lights + CarlaDataProvider._traffic_light_map.clear() + for traffic_light in CarlaDataProvider._world.get_actors().filter("*traffic_light*"): + if traffic_light not in list(CarlaDataProvider._traffic_light_map): + CarlaDataProvider._traffic_light_map[traffic_light] = traffic_light.get_transform() + else: + raise KeyError( + "Traffic light '{}' already registered. Cannot register twice!".format( + traffic_light.id + ) + ) + + @staticmethod + def generate_spawn_points(): + """Generate spawn points for the current map.""" + spawn_points = list(CarlaDataProvider.get_map(CarlaDataProvider._world).get_spawn_points()) + CarlaDataProvider._rng.shuffle(spawn_points) + CarlaDataProvider._spawn_points = spawn_points + CarlaDataProvider._spawn_index = 0 + + @staticmethod + def check_road_length(wp, length: float): + waypoint_separation = 5 + + cur_len = 0 + road_id, lane_id = wp.road_id, wp.lane_id + while True: + wps = wp.next(waypoint_separation) + # The same road_id and lane_id，judged to be in the same section to be tested + next_wp = None + for p in wps: + if p.road_id == road_id and p.lane_id == lane_id: + next_wp = p + break + if next_wp is None: + break + cur_len += waypoint_separation + if cur_len >= length: + return True + wp = next_wp + return False + + @staticmethod + def get_road_lanes(wp): + if wp.is_junction: + return [] + # find the most left lane's waypoint + + lane_id_set = set() + pre_left = wp + while wp and wp.lane_type == carla.LaneType.Driving: + if wp.lane_id in lane_id_set: + break + lane_id_set.add(wp.lane_id) + + # carla bug: get_left_lane Return error，and never Return none. It's a infinite loop. + pre_left = wp + wp = wp.get_left_lane() + + # # Store data from the left lane to the right lane + # # list, key=laneid, value=waypoint + lane_list = [] + lane_id_set.clear() + wp = pre_left + while wp and wp.lane_type == carla.LaneType.Driving: + if wp.lane_id in lane_id_set: + break + lane_id_set.add(wp.lane_id) + + lane_list.append(wp) + + # carla bug: Return error, never return none, endless loop + wp = wp.get_right_lane() + + return lane_list + + @staticmethod + def get_road_lane_cnt(wp): + lanes = CarlaDataProvider.get_road_lanes(wp) + return len(lanes) + + @staticmethod + def get_waypoint_by_laneid(lane_num: int): + if CarlaDataProvider._spawn_points is None: + CarlaDataProvider.generate_spawn_points() + + if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): + print("No more spawn points to use") + return None + else: + pos = CarlaDataProvider._spawn_points[ + CarlaDataProvider._spawn_index + ] # pylint: disable=unsubscriptable-object + CarlaDataProvider._spawn_index += 1 + wp = CarlaDataProvider.get_map().get_waypoint( + pos.location, project_to_road=True, lane_type=carla.LaneType.Driving + ) + + road_lanes = CarlaDataProvider.get_road_lanes(wp) + + lane = int(float(lane_num)) + if lane > len(road_lanes): + return None + else: + return road_lanes[lane - 1] + + # cspell:ignore rolename + @staticmethod + def create_blueprint( + model, rolename="scenario", color=None, actor_category="car", attribute_filter=None + ): + """Set up the blueprint of an actor given its model and other relevant parameters.""" + + def check_attribute_value(blueprint, name, value): + """Check if the blueprint has that attribute with that value.""" + if not blueprint.has_attribute(name): + return False + + attribute_type = blueprint.get_attribute(key).type + if attribute_type == carla.ActorAttributeType.Bool: + return blueprint.get_attribute(name).as_bool() == value + elif attribute_type == carla.ActorAttributeType.Int: + return blueprint.get_attribute(name).as_int() == value + elif attribute_type == carla.ActorAttributeType.Float: + return blueprint.get_attribute(name).as_float() == value + elif attribute_type == carla.ActorAttributeType.String: + return blueprint.get_attribute(name).as_str() == value + + return False + + # cspell:ignore carlacola carlamotors + _actor_blueprint_categories = { + "car": "vehicle.tesla.model3", + "van": "vehicle.volkswagen.t2", + "truck": "vehicle.carlamotors.carlacola", + "trailer": "", + "semitrailer": "", + "bus": "vehicle.volkswagen.t2", + "motorbike": "vehicle.kawasaki.ninja", + "bicycle": "vehicle.diamondback.century", + "train": "", + "tram": "", + "pedestrian": "walker.pedestrian.0001", + } + + # Set the model + try: + blueprints = CarlaDataProvider._blueprint_library.filter(model) + if attribute_filter is not None: + for key, value in attribute_filter.items(): + blueprints = [x for x in blueprints if check_attribute_value(x, key, value)] + + blueprint = CarlaDataProvider._rng.choice(blueprints) + except ValueError: + # The model is not part of the blueprint library. Let's take a default one for the given category + bp_filter = "vehicle.*" + new_model = _actor_blueprint_categories[actor_category] + if new_model != "": + bp_filter = new_model + print( + "WARNING: Actor model {} not available. Using instead {}".format(model, new_model) + ) + blueprint = CarlaDataProvider._rng.choice( + CarlaDataProvider._blueprint_library.filter(bp_filter) + ) + + # Set the color + if color: + if not blueprint.has_attribute("color"): + print( + "WARNING: Cannot set Color ({}) for actor {} due to missing blueprint attribute".format( + color, blueprint.id + ) + ) + else: + default_color_rgba = blueprint.get_attribute("color").as_color() + default_color = "({}, {}, {})".format( + default_color_rgba.r, default_color_rgba.g, default_color_rgba.b + ) + try: + blueprint.set_attribute("color", color) + except ValueError: + # Color can't be set for this vehicle + print( + "WARNING: Color ({}) cannot be set for actor {}. Using instead: ({})".format( + color, blueprint.id, default_color + ) + ) + blueprint.set_attribute("color", default_color) + else: + if blueprint.has_attribute("color") and rolename != "hero": + color = CarlaDataProvider._rng.choice( + blueprint.get_attribute("color").recommended_values + ) + blueprint.set_attribute("color", color) + + # Make pedestrians mortal + if blueprint.has_attribute("is_invincible"): + blueprint.set_attribute("is_invincible", "false") + + # Set the rolename + if blueprint.has_attribute("role_name"): + blueprint.set_attribute("role_name", rolename) + + return blueprint + + @staticmethod + def handle_actor_batch(batch, tick=True): + """Forward a CARLA command batch to spawn actors to CARLA, and gather the responses.""" + sync_mode = CarlaDataProvider.is_sync_mode() + actors = [] + + if CarlaDataProvider._client: + responses = CarlaDataProvider._client.apply_batch_sync(batch, sync_mode and tick) + else: + raise ValueError("class member 'client'' not initialized yet") + + # Wait (or not) for the actors to be spawned properly before we do anything + if not tick: + pass + elif CarlaDataProvider.is_runtime_init_mode(): + CarlaDataProvider._world.wait_for_tick() + elif sync_mode: + CarlaDataProvider._world.tick() + else: + CarlaDataProvider._world.wait_for_tick() + + actor_ids = [r.actor_id for r in responses if not r.error] + for r in responses: + if r.error: + print("WARNING: Not all actors were spawned") + break + actors = list(CarlaDataProvider._world.get_actors(actor_ids)) + return actors + + @staticmethod + def request_new_actor( + model, + spawn_point, + rolename="scenario", + autopilot=False, + random_location=False, + color=None, + actor_category="car", + attribute_filter=None, + tick=True, + ): + """Create a new actor, returning it if successful (None otherwise).""" + blueprint = CarlaDataProvider.create_blueprint( + model, rolename, color, actor_category, attribute_filter + ) + + if random_location: + actor = None + while not actor: + spawn_point = CarlaDataProvider._rng.choice(CarlaDataProvider._spawn_points) + actor = CarlaDataProvider._world.try_spawn_actor(blueprint, spawn_point) + + else: + # For non prop models, slightly lift the actor to avoid collisions with the ground + z_offset = 0.2 if "prop" not in model else 0 + + # DO NOT USE spawn_point directly, as this will modify spawn_point permanently + _spawn_point = carla.Transform(carla.Location(), spawn_point.rotation) + _spawn_point.location.x = spawn_point.location.x + _spawn_point.location.y = spawn_point.location.y + _spawn_point.location.z = spawn_point.location.z + z_offset + actor = CarlaDataProvider._world.try_spawn_actor(blueprint, _spawn_point) + + if actor is None: + print( + "WARNING: Cannot spawn actor {} at position {}".format(model, spawn_point.location) + ) + return None + + # De/activate the autopilot of the actor if it belongs to vehicle + if autopilot: + if isinstance(actor, carla.Vehicle): + actor.set_autopilot(autopilot, CarlaDataProvider._traffic_manager_port) + else: + print("WARNING: Tried to set the autopilot of a non vehicle actor") + + # Wait for the actor to be spawned properly before we do anything + if not tick: + pass + elif CarlaDataProvider.is_runtime_init_mode(): + CarlaDataProvider._world.wait_for_tick() + elif CarlaDataProvider.is_sync_mode(): + CarlaDataProvider._world.tick() + else: + CarlaDataProvider._world.wait_for_tick() + + if actor is None: + return None + + CarlaDataProvider._carla_actor_pool[actor.id] = actor + CarlaDataProvider.register_actor(actor, spawn_point) + return actor + + @staticmethod + def request_new_actors(actor_list, attribute_filter=None, tick=True): + """Series of actor in batch. If this was successful, the new actors are returned, None otherwise.""" + SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name + PhysicsCommand = carla.command.SetSimulatePhysics # pylint: disable=invalid-name + FutureActor = carla.command.FutureActor # pylint: disable=invalid-name + ApplyTransform = carla.command.ApplyTransform # pylint: disable=invalid-name + SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name + SetVehicleLightState = carla.command.SetVehicleLightState # pylint: disable=invalid-name + + batch = [] + actors = [] + + CarlaDataProvider.generate_spawn_points() + + for actor in actor_list: + # Get the blueprint + blueprint = CarlaDataProvider.create_blueprint( + actor.model, actor.rolename, actor.color, actor.category, attribute_filter + ) + + # Get the spawn point + transform = actor.transform + if not transform: + continue + if actor.random_location: + if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): + print("No more spawn points to use") + break + else: + _spawn_point = CarlaDataProvider._spawn_points[ + CarlaDataProvider._spawn_index + ] # pylint: disable=unsubscriptable-object + CarlaDataProvider._spawn_index += 1 + + else: + _spawn_point = carla.Transform() + _spawn_point.rotation = transform.rotation + _spawn_point.location.x = transform.location.x + _spawn_point.location.y = transform.location.y + if blueprint.has_tag("walker"): + # On imported OpenDRIVE maps, spawning of pedestrians can fail. + # By increasing the z-value the chances of success are increased. + map_name = CarlaDataProvider._map.name.split("/")[-1] + if not map_name.startswith("OpenDrive"): + _spawn_point.location.z = transform.location.z + 0.2 + else: + _spawn_point.location.z = transform.location.z + 0.8 + else: + _spawn_point.location.z = transform.location.z + 0.2 + + # Get the command + command = SpawnActor(blueprint, _spawn_point) + command.then( + SetAutopilot(FutureActor, actor.autopilot, CarlaDataProvider._traffic_manager_port) + ) + + if ( + actor.args is not None + and "physics" in actor.args + and actor.args["physics"] == "off" + ): + command.then(ApplyTransform(FutureActor, _spawn_point)).then( + PhysicsCommand(FutureActor, False) + ) + elif actor.category == "misc": + command.then(PhysicsCommand(FutureActor, True)) + if actor.args is not None and "lights" in actor.args and actor.args["lights"] == "on": + command.then(SetVehicleLightState(FutureActor, carla.VehicleLightState.All)) + + batch.append(command) + + actors = CarlaDataProvider.handle_actor_batch(batch, tick) + + if not actors: + return None + + for actor in actors: + if actor is None: + continue + CarlaDataProvider._carla_actor_pool[actor.id] = actor + CarlaDataProvider.register_actor(actor, _spawn_point) + + return actors + + @staticmethod + def request_new_batch_actors( + model, + amount, + spawn_points, + autopilot=False, + random_location=False, + rolename="scenario", + attribute_filter=None, + tick=True, + ): + SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name + SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name + FutureActor = carla.command.FutureActor # pylint: disable=invalid-name + + CarlaDataProvider.generate_spawn_points() + + batch = [] + + for i in range(amount): + # Get vehicle by model + blueprint = CarlaDataProvider.create_blueprint( + model, rolename, attribute_filter=attribute_filter + ) + + if random_location: + if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): + print( + "No more spawn points to use. Spawned {} actors out of {}".format( + i + 1, amount + ) + ) + break + else: + spawn_point = CarlaDataProvider._spawn_points[ + CarlaDataProvider._spawn_index + ] # pylint: disable=unsubscriptable-object + CarlaDataProvider._spawn_index += 1 + else: + try: + spawn_point = spawn_points[i] + except IndexError: + print("The amount of spawn points is lower than the amount of vehicles spawned") + break + + if spawn_point: + batch.append( + SpawnActor(blueprint, spawn_point).then( + SetAutopilot( + FutureActor, autopilot, CarlaDataProvider._traffic_manager_port + ) + ) + ) + + actors = CarlaDataProvider.handle_actor_batch(batch, tick) + for actor in actors: + if actor is None: + continue + CarlaDataProvider._carla_actor_pool[actor.id] = actor + CarlaDataProvider.register_actor(actor, spawn_point) + + return actors + + @staticmethod + def get_actors(): + """Return list of actors and their ids.""" + return iteritems(CarlaDataProvider._carla_actor_pool) + + @staticmethod + def actor_id_exists(actor_id): + """Check if a certain id is still at the simulation.""" + if actor_id in CarlaDataProvider._carla_actor_pool: + return True + + return False + + @staticmethod + def get_hero_actor(): + """Get the actor object of the hero actor if it exists, Return none otherwise.""" + for actor_id in CarlaDataProvider._carla_actor_pool: + if CarlaDataProvider._carla_actor_pool[actor_id].attributes["role_name"] == "hero": + return CarlaDataProvider._carla_actor_pool[actor_id] + return None + + @staticmethod + def get_actor_by_id(actor_id): + """Get an actor from the pool by using its ID. If the actor does not exist, None is returned.""" + print(CarlaDataProvider._carla_actor_pool) + if actor_id in CarlaDataProvider._carla_actor_pool: + return CarlaDataProvider._carla_actor_pool[actor_id] + + print("Non-existing actor id {}".format(actor_id)) + return None + + @staticmethod + def get_actor_by_name(role_name: str): + for actor_id in CarlaDataProvider._carla_actor_pool: + if CarlaDataProvider._carla_actor_pool[actor_id].attributes["role_name"] == role_name: + return CarlaDataProvider._carla_actor_pool[actor_id] + print(f"Non-existing actor name {role_name}") + return None + + @staticmethod + def remove_actor_by_id(actor_id): + """Remove an actor from the pool using its ID.""" + if actor_id in CarlaDataProvider._carla_actor_pool: + CarlaDataProvider._carla_actor_pool[actor_id].destroy() + CarlaDataProvider._carla_actor_pool[actor_id] = None + CarlaDataProvider._carla_actor_pool.pop(actor_id) + else: + print("Trying to remove a non-existing actor id {}".format(actor_id)) + + @staticmethod + def is_sync_mode(): + """Return true if synchronous mode is used.""" + return CarlaDataProvider._sync_flag + + @staticmethod + def remove_actors_in_surrounding(location, distance): + """Remove all actors from the pool that are closer than distance to the provided location.""" + for actor_id in CarlaDataProvider._carla_actor_pool.copy(): + if ( + CarlaDataProvider._carla_actor_pool[actor_id].get_location().distance(location) + < distance + ): + CarlaDataProvider._carla_actor_pool[actor_id].destroy() + CarlaDataProvider._carla_actor_pool.pop(actor_id) + + # Remove all keys with None values + CarlaDataProvider._carla_actor_pool = dict( + {k: v for k, v in CarlaDataProvider._carla_actor_pool.items() if v} + ) + + @staticmethod + def get_traffic_manager_port(): + """Get the port of the traffic manager.""" + return CarlaDataProvider._traffic_manager_port + + @staticmethod + def set_traffic_manager_port(tm_port): + """Set the port to use for the traffic manager.""" + CarlaDataProvider._traffic_manager_port = tm_port + + @staticmethod + def cleanup(): + """Cleanup and remove all entries from all dictionaries.""" + DestroyActor = carla.command.DestroyActor # pylint: disable=invalid-name + batch = [] + + for actor_id in CarlaDataProvider._carla_actor_pool.copy(): + actor = CarlaDataProvider._carla_actor_pool[actor_id] + if actor is not None and actor.is_alive: + batch.append(DestroyActor(actor)) + + if CarlaDataProvider._client: + try: + CarlaDataProvider._client.apply_batch_sync(batch) + except RuntimeError as e: + if "time-out" in str(e): + pass + else: + raise e + + CarlaDataProvider._actor_velocity_map.clear() + CarlaDataProvider._actor_location_map.clear() + CarlaDataProvider._actor_transform_map.clear() + CarlaDataProvider._traffic_light_map.clear() + CarlaDataProvider._map = None + CarlaDataProvider._world = None + CarlaDataProvider._sync_flag = False + CarlaDataProvider._ego_vehicle_route = None + CarlaDataProvider._all_actors = None + CarlaDataProvider._carla_actor_pool = {} + CarlaDataProvider._client = None + CarlaDataProvider._spawn_points = None + CarlaDataProvider._spawn_index = 0 + CarlaDataProvider._rng = random.RandomState(CarlaDataProvider._random_seed) + CarlaDataProvider._runtime_init_flag = False + + @property + def world(self): + return self._world + + +class GameTime(object): + """Provides access to the CARLA game time.""" + + _current_game_time = 0.0 # Elapsed game time after starting this Timer + _carla_time = 0.0 + _last_frame = 0 + _platform_timestamp = 0 + _init = False + + @staticmethod + def on_carla_tick(timestamp): + """Handle the callback receiving the CARLA time. Update time only when the frame is more recent than the last frame.""" + if GameTime._last_frame < timestamp.frame: + frames = timestamp.frame - GameTime._last_frame if GameTime._init else 1 + GameTime._current_game_time += timestamp.delta_seconds * frames + GameTime._last_frame = timestamp.frame + GameTime._platform_timestamp = datetime.datetime.now() + GameTime._init = True + GameTime._carla_time = timestamp.elapsed_seconds + + @staticmethod + def restart(): + """Reset game timer to 0.""" + GameTime._current_game_time = 0.0 + GameTime._carla_time = 0.0 + GameTime._last_frame = 0 + GameTime._init = False + + @staticmethod + def get_time(): + """Return elapsed game time.""" + return GameTime._current_game_time + + @staticmethod + def get_carla_time(): + """Return elapsed game time.""" + return GameTime._carla_time + + @staticmethod + def get_wall_clock_time(): + """Return elapsed game time.""" + return GameTime._platform_timestamp + + @staticmethod + def get_frame(): + """Return elapsed game time.""" + return GameTime._last_frame diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_utils.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_utils.py new file mode 100644 index 0000000000000..72eb176515f73 --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_utils.py @@ -0,0 +1,109 @@ +import ctypes +import math +import struct +import sys + +import carla +from geometry_msgs.msg import Point +from geometry_msgs.msg import Quaternion +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from transforms3d.euler import euler2quat +from transforms3d.euler import quat2euler + + +def _get_struct_fmt(is_bigendian, fields, field_names=None): + _DATATYPES = {} + _DATATYPES[PointField.INT8] = ("b", 1) + _DATATYPES[PointField.UINT8] = ("B", 1) + _DATATYPES[PointField.INT16] = ("h", 2) + _DATATYPES[PointField.UINT16] = ("H", 2) + _DATATYPES[PointField.INT32] = ("i", 4) + _DATATYPES[PointField.UINT32] = ("I", 4) + _DATATYPES[PointField.FLOAT32] = ("f", 4) + _DATATYPES[PointField.FLOAT64] = ("d", 8) + + fmt = ">" if is_bigendian else "<" + + offset = 0 + for field in ( + f + for f in sorted(fields, key=lambda f: f.offset) + if field_names is None or f.name in field_names + ): + if offset < field.offset: + fmt += "x" * (field.offset - offset) + offset = field.offset + if field.datatype not in _DATATYPES: + print("Skipping unknown PointField datatype [{}]" % field.datatype, file=sys.stderr) + else: + datatype_fmt, datatype_length = _DATATYPES[field.datatype] + fmt += field.count * datatype_fmt + offset += field.count * datatype_length + + return fmt + + +def create_cloud(header, fields, points): + """Create a L{sensor_msgs.msg.PointCloud2} message with different datatype (Modified create_cloud function).""" + cloud_struct = struct.Struct(_get_struct_fmt(False, fields)) + + buff = ctypes.create_string_buffer(cloud_struct.size * len(points)) + + point_step, pack_into = cloud_struct.size, cloud_struct.pack_into + offset = 0 + for p in points: + pack_into(buff, offset, *p) + offset += point_step + + return PointCloud2( + header=header, + height=1, + width=len(points), + is_dense=False, + is_bigendian=False, + fields=fields, + point_step=cloud_struct.size, + row_step=cloud_struct.size * len(points), + data=buff.raw, + ) + + +def carla_location_to_ros_point(carla_location): + """Convert a carla location to a ROS point.""" + ros_point = Point() + ros_point.x = carla_location.x + ros_point.y = -carla_location.y + ros_point.z = carla_location.z + + return ros_point + + +def carla_rotation_to_ros_quaternion(carla_rotation): + """Convert a carla rotation to a ROS quaternion.""" + roll = math.radians(carla_rotation.roll) + pitch = -math.radians(carla_rotation.pitch) + yaw = -math.radians(carla_rotation.yaw) + quat = euler2quat(roll, pitch, yaw) + ros_quaternion = Quaternion(w=quat[0], x=quat[1], y=quat[2], z=quat[3]) + + return ros_quaternion + + +def ros_quaternion_to_carla_rotation(ros_quaternion): + """Convert ROS quaternion to carla rotation.""" + roll, pitch, yaw = quat2euler( + [ros_quaternion.w, ros_quaternion.x, ros_quaternion.y, ros_quaternion.z] + ) + + return carla.Rotation( + roll=math.degrees(roll), pitch=-math.degrees(pitch), yaw=-math.degrees(yaw) + ) + + +def ros_pose_to_carla_transform(ros_pose): + """Convert ROS pose to carla transform.""" + return carla.Transform( + carla.Location(ros_pose.position.x, -ros_pose.position.y, ros_pose.position.z), + ros_quaternion_to_carla_rotation(ros_pose.orientation), + ) diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py new file mode 100644 index 0000000000000..ed036a47ea9fe --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py @@ -0,0 +1,231 @@ +# 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.sr/bin/env python + +from __future__ import print_function + +import logging +from queue import Empty +from queue import Queue + +import carla +import numpy as np + +from .carla_data_provider import CarlaDataProvider + + +# Sensor Wrapper for Agent +class SensorReceivedNoData(Exception): + """Exceptions when no data received from the sensors.""" + + +class GenericMeasurement(object): + def __init__(self, data, frame): + self.data = data + self.frame = frame + + +class CallBack(object): + def __init__(self, tag, sensor, data_provider): + self._tag = tag + self._data_provider = data_provider + + self._data_provider.register_sensor(tag, sensor) + + def __call__(self, data): + if isinstance(data, carla.Image): + self._parse_image_cb(data, self._tag) + elif isinstance(data, carla.LidarMeasurement): + self._parse_lidar_cb(data, self._tag) + elif isinstance(data, carla.GnssMeasurement): + self._parse_gnss_cb(data, self._tag) + elif isinstance(data, carla.IMUMeasurement): + self._parse_imu_cb(data, self._tag) + elif isinstance(data, GenericMeasurement): + self._parse_pseudo_sensor(data, self._tag) + else: + logging.error("No callback method for this sensor.") + + # Parsing CARLA physical Sensors + def _parse_image_cb(self, image, tag): + self._data_provider.update_sensor(tag, image, image.frame) + + def _parse_lidar_cb(self, lidar_data, tag): + self._data_provider.update_sensor(tag, lidar_data, lidar_data.frame) + + def _parse_imu_cb(self, imu_data, tag): + self._data_provider.update_sensor(tag, imu_data, imu_data.frame) + + def _parse_gnss_cb(self, gnss_data, tag): + array = np.array( + [gnss_data.latitude, gnss_data.longitude, gnss_data.altitude], dtype=np.float64 + ) + self._data_provider.update_sensor(tag, array, gnss_data.frame) + + def _parse_pseudo_sensor(self, package, tag): + self._data_provider.update_sensor(tag, package.data, package.frame) + + +class SensorInterface(object): + def __init__(self): + self._sensors_objects = {} + self._new_data_buffers = Queue() + self._queue_timeout = 10 + self.tag = "" + + def register_sensor(self, tag, sensor): + self.tag = tag + if tag in self._sensors_objects: + raise ValueError(f"Duplicated sensor tag [{tag}]") + + self._sensors_objects[tag] = sensor + + def update_sensor(self, tag, data, timestamp): + if tag not in self._sensors_objects: + raise ValueError(f"Sensor with tag [{tag}] has not been created") + + self._new_data_buffers.put((tag, timestamp, data)) + + def get_data(self): + try: + data_dict = {} + while len(data_dict.keys()) < len(self._sensors_objects.keys()): + sensor_data = self._new_data_buffers.get(True, self._queue_timeout) + data_dict[sensor_data[0]] = (sensor_data[1], sensor_data[2]) + except Empty: + raise SensorReceivedNoData( + f"Sensor with tag [{self.tag}] took too long to send its data" + ) + + return data_dict + + +# Sensor Wrapper + + +class SensorWrapper(object): + _agent = None + _sensors_list = [] + + def __init__(self, agent): + self._agent = agent + + def __call__(self): + return self._agent() + + def setup_sensors(self, vehicle, debug_mode=False): + """Create and attach the sensor defined in objects.json.""" + bp_library = CarlaDataProvider.get_world().get_blueprint_library() + + for sensor_spec in self._agent.sensors["sensors"]: + bp = bp_library.find(str(sensor_spec["type"])) + + if sensor_spec["type"].startswith("sensor.camera"): + bp.set_attribute("image_size_x", str(sensor_spec["image_size_x"])) + bp.set_attribute("image_size_y", str(sensor_spec["image_size_y"])) + bp.set_attribute("fov", str(sensor_spec["fov"])) + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"], + roll=sensor_spec["spawn_point"]["roll"], + yaw=sensor_spec["spawn_point"]["yaw"], + ) + + elif sensor_spec["type"].startswith("sensor.lidar"): + bp.set_attribute("range", str(sensor_spec["range"])) + bp.set_attribute("rotation_frequency", str(sensor_spec["rotation_frequency"])) + bp.set_attribute("channels", str(sensor_spec["channels"])) + bp.set_attribute("upper_fov", str(sensor_spec["upper_fov"])) + bp.set_attribute("lower_fov", str(sensor_spec["lower_fov"])) + bp.set_attribute("points_per_second", str(sensor_spec["points_per_second"])) + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"], + roll=sensor_spec["spawn_point"]["roll"], + yaw=sensor_spec["spawn_point"]["yaw"], + ) + + elif sensor_spec["type"].startswith("sensor.other.gnss"): + bp.set_attribute("noise_alt_stddev", str(0.0)) + bp.set_attribute("noise_lat_stddev", str(0.0)) + bp.set_attribute("noise_lon_stddev", str(0.0)) + bp.set_attribute("noise_alt_bias", str(0.0)) + bp.set_attribute("noise_lat_bias", str(0.0)) + bp.set_attribute("noise_lon_bias", str(0.0)) + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"], + roll=sensor_spec["spawn_point"]["roll"], + yaw=sensor_spec["spawn_point"]["yaw"], + ) + + elif sensor_spec["type"].startswith("sensor.other.imu"): + bp.set_attribute("noise_accel_stddev_x", str(0.0)) + bp.set_attribute("noise_accel_stddev_y", str(0.0)) + bp.set_attribute("noise_accel_stddev_z", str(0.0)) + bp.set_attribute("noise_gyro_stddev_x", str(0.0)) + bp.set_attribute("noise_gyro_stddev_y", str(0.0)) + bp.set_attribute("noise_gyro_stddev_z", str(0.0)) + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"], + roll=sensor_spec["spawn_point"]["roll"], + yaw=sensor_spec["spawn_point"]["yaw"], + ) + + elif sensor_spec["type"].startswith("sensor.pseudo.*"): + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"] + 0.001, + roll=sensor_spec["spawn_point"]["roll"] - 0.015, + yaw=sensor_spec["spawn_point"]["yaw"] + 0.0364, + ) + + # create sensor + sensor_transform = carla.Transform(sensor_location, sensor_rotation) + sensor = CarlaDataProvider.get_world().spawn_actor(bp, sensor_transform, vehicle) + # setup callback + sensor.listen(CallBack(sensor_spec["id"], sensor, self._agent.sensor_interface)) + self._sensors_list.append(sensor) + + # Tick once to spawn the sensors + CarlaDataProvider.get_world().tick() + + def cleanup(self): + """Cleanup sensors.""" + for i, _ in enumerate(self._sensors_list): + if self._sensors_list[i] is not None: + self._sensors_list[i].stop() + self._sensors_list[i].destroy() + self._sensors_list[i] = None + self._sensors_list = [] 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/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 376553aa62917..07e7e6b5ad169 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -21,6 +21,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp + src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp src/simple_planning_simulator/utils/csv_loader.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learning_based_vehicle_model_INCLUDE_DIRS}) @@ -41,4 +42,4 @@ if(BUILD_TESTING) ) endif() -ament_auto_package(INSTALL_TO_SHARE param launch) +ament_auto_package(INSTALL_TO_SHARE param data launch test) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index c3d02da64b882..d44264035ecc4 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -65,6 +65,7 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_ACC_GEARED_WO_FALL_GUARD` - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. - `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learning_based_vehicle_model). +- `ACTUATION_CMD`: A simulator model that receives `ACTUATION_CMD`. In this case, the `raw_vehicle_cmd_converter` is also launched. The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. @@ -122,6 +123,22 @@ default, 0.00, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 1 ![acceleration_map](./media/acceleration_map.svg) +##### ACTUATION_CMD model + +The simple_planning_simulator usually operates by receiving Control commands, but when the `ACTUATION_CMD` model is selected, it receives Actuation commands instead of Control commands. This model can simulate the motion using the vehicle command that is actually sent to the real vehicle. Therefore, when this model is selected, the `raw_vehicle_cmd_converter` is also launched. + +The parameters used in the ACTUATION_CMD are as follows. + +| Name | Type | Description | unit | +| :------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--- | +| accel_time_delay | double | dead time for the acceleration input | [s] | +| accel_time_constant | double | time constant of the 1st-order acceleration dynamics | [s] | +| brake_time_delay | double | dead time for the brake input | [s] | +| brake_time_constant | double | time constant of the 1st-order brake dynamics | [s] | +| convert_accel_cmd | bool | If true, it is assumed that the command is received converted to an accel actuation value, and it is converted back to acceleration value inside the simulator. | [-] | +| convert_brake_cmd | bool | If true, it is assumed that the command is received converted to a brake actuation value, and it is converted back to acceleration value inside the simulator. | [-] | +| convert_steer_cmd | bool | If true, it is assumed that the command is received converted to a steer actuation value, and it is converted back to steer rate value inside the simulator. | [-] | + _Note_: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a _delay_ model. The definition of the _time constant_ is the time it takes for the step response to rise up to 63% of its final value. The _deadtime_ is a delay in the response to a control input. diff --git a/simulator/simple_planning_simulator/data/actuation_cmd_map/accel_map.csv b/simulator/simple_planning_simulator/data/actuation_cmd_map/accel_map.csv new file mode 100644 index 0000000000000..32e639cad3373 --- /dev/null +++ b/simulator/simple_planning_simulator/data/actuation_cmd_map/accel_map.csv @@ -0,0 +1,7 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 +0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 +0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 +0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 +0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 diff --git a/simulator/simple_planning_simulator/data/actuation_cmd_map/brake_map.csv b/simulator/simple_planning_simulator/data/actuation_cmd_map/brake_map.csv new file mode 100644 index 0000000000000..adf2c80950bb1 --- /dev/null +++ b/simulator/simple_planning_simulator/data/actuation_cmd_map/brake_map.csv @@ -0,0 +1,10 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51 +0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96 +0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633 +0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 +0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 +0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 +0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 +0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955 diff --git a/simulator/simple_planning_simulator/data/actuation_cmd_map/steer_map.csv b/simulator/simple_planning_simulator/data/actuation_cmd_map/steer_map.csv new file mode 100644 index 0000000000000..3da0e0cd54f2d --- /dev/null +++ b/simulator/simple_planning_simulator/data/actuation_cmd_map/steer_map.csv @@ -0,0 +1,14 @@ +default,-0.6,-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4,0.5,0.6 +-12,-0.29,-0.3234236132,-0.3468991362,-0.37274847,-0.4288688461,-0.4696573,-0.4781014157,-0.49,-0.51,-0.52,-0.53,-0.54,-0.55 +-10,-0.2,-0.2451308686,-0.2667782734,-0.3090752469,-0.3575200568,-0.3874868999,-0.4041493831,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47 +-8,-0.1,-0.1586739777,-0.2020698818,-0.2317654357,-0.2778998646,-0.3029839842,-0.3188172953,-0.32,-0.33,-0.34,-0.35,-0.36,-0.37 +-6,-0.0404761584,-0.0806574159,-0.1208386734,-0.1559621241,-0.1888907059,-0.210790018,-0.2322336736,-0.24,-0.25,-0.26,-0.27,-0.28,-0.29 +-4,0.01,-0.01428569928,-0.04,-0.06805018099,-0.09718925222,-0.1189509092,-0.1361293637,-0.14,-0.153044463,-0.1688321844,-0.18,-0.2,-0.22 +-2,0.06,0.05,0.03,0.01,-0.006091655083,-0.03546033903,-0.05319488695,-0.06097627388,-0.07576251508,-0.09165178816,-0.1,-0.12,-0.14 +0,0.11,0.09,0.07,0.05,0.03,0.01,-0.0004106386541,-0.01,-0.03,-0.05,-0.07,-0.09,-0.11 +2,0.15,0.13,0.11,0.09,0.07812978496,0.06023545297,0.04386326928,0.02314813566,0.003779338416,-0.01390526686,-0.0324840879,-0.05106290894,-0.06964172998 +4,0.24,0.22,0.2,0.1839795042,0.1649754952,0.1455879759,0.1274837062,0.1020632549,0.0861964856,0.06971503353,0.04541294018,0.01308869356,-0.01923555307 +6,0.33,0.31,0.29,0.274963497,0.2447295892,0.2337353319,0.2126138313,0.1767700907,0.1578757866,0.1440584148,0.1136331403,0.0827443595,0.05185557872 +8,0.43,0.41,0.39,0.3714915121,0.3299578073,0.3156403746,0.2997845963,0.2500495071,0.2339668568,0.2153133621,0.1815127859,0.156071251,0.1306297162 +10,0.5,0.49,0.48,0.4629455165,0.4210353203,0.3977027236,0.3865034288,0.3250957262,0.312237913,0.2885123051,0.2474220915,0.2123900615,0.1773580315 +12,0.56,0.54,0.53,0.5151036525,0.5046070554,0.4897214196,0.4687756354,0.4036994672,0.3812674227,0.3496883008,0.32482655,0.2783538423,0.2318811345 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 86e4346770f0c..0ed603960a6c5 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -44,6 +44,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/imu.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" +#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" #include #include @@ -52,6 +53,7 @@ #include #include #include +#include #include namespace simulation @@ -83,6 +85,7 @@ using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using tier4_external_api_msgs::srv::InitializePose; +using tier4_vehicle_msgs::msg::ActuationCommandStamped; class DeltaTime { @@ -115,6 +118,8 @@ class MeasurementNoiseGenerator std::shared_ptr> steer_dist_; }; +using InputCommand = std::variant; + class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node { public: @@ -138,7 +143,6 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_manual_gear_cmd_; rclcpp::Subscription::SharedPtr sub_turn_indicators_cmd_; rclcpp::Subscription::SharedPtr sub_hazard_lights_cmd_; - rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_init_pose_; @@ -146,6 +150,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Subscription::SharedPtr sub_engage_; + // todo + rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_actuation_cmd_; + rclcpp::Service::SharedPtr srv_mode_req_; rclcpp::CallbackGroup::SharedPtr group_api_service_; @@ -189,6 +197,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node bool is_initialized_ = false; //!< @brief flag to check the initial position is set bool add_measurement_noise_ = false; //!< @brief flag to add measurement noise + InputCommand current_input_command_{}; + DeltaTime delta_time_{}; //!< @brief to calculate delta time MeasurementNoiseGenerator measurement_noise_{}; //!< @brief for measurement noise @@ -206,15 +216,20 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node DELAY_STEER_VEL = 5, DELAY_STEER_MAP_ACC_GEARED = 6, LEARNED_STEER_VEL = 7, - DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8 + DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8, + ACTUATION_CMD = 9 } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer + void set_input(const InputCommand & cmd, const double acc_by_slope); + /** * @brief set input steering, velocity, and acceleration of the vehicle model */ void set_input(const Control & cmd, const double acc_by_slope); + void set_input(const ActuationCommandStamped & cmd, const double acc_by_slope); + /** * @brief set current_vehicle_state_ with received message */ @@ -295,7 +310,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief initialize vehicle_model_ptr */ - void initialize_vehicle_model(); + void initialize_vehicle_model(const std::string & vehicle_model_type_str); /** * @brief add measurement noise diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp index fc71837541b83..a8851640fb62b 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp @@ -34,8 +34,7 @@ class CSVLoader static Map getMap(const Table & table); static std::vector getRowIndex(const Table & table); static std::vector getColumnIndex(const Table & table); - static double clampValue( - const double val, const std::vector & ranges, const std::string & name); + static double clampValue(const double val, const std::vector & ranges); private: std::string csv_path_; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index 2d38ef31498b6..52c3825ae1888 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -15,6 +15,7 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ +#include "simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp new file mode 100644 index 0000000000000..1385a509b8dca --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp @@ -0,0 +1,209 @@ +// 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 SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" +#include "interpolation/linear_interpolation.hpp" +#include "simple_planning_simulator/utils/csv_loader.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include +#include +#include +#include + +/** + * @class ActuationMap + * @brief class to convert actuation command + */ +class ActuationMap +{ +public: + /** + * @brief read actuation map from csv file + * @param [in] csv_path path to csv file + * @param [in] validation flag to validate data + * @return true if success to read + */ + bool readActuationMapFromCSV(const std::string & csv_path, const bool validation = false); + double getControlCommand(const double actuation, const double state) const; + +private: + std::vector state_index_; // e.g. velocity, steering + std::vector actuation_index_; + std::vector> actuation_map_; +}; + +/** + * @class SimModelActuationCmd + * @brief class to handle vehicle model with actuation command + */ +class SimModelActuationCmd : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] steer_lim steering limit [rad] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] + * @param [in] wheelbase vehicle wheelbase length [m] + * @param [in] dt delta time information to set input buffer for delay + * @param [in] accel_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] brake_delay time delay for brake command [s] + * @param [in] brake_time_constant time constant for 1D model of brake dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] steer_bias steering bias [rad] + * @param [in] convert_accel_cmd flag to convert accel command + * @param [in] convert_brake_cmd flag to convert brake command + * @param [in] convert_steer_cmd flag to convert steer command + * @param [in] accel_map_path path to csv file for accel conversion map + * @param [in] brake_map_path path to csv file for brake conversion map + * @param [in] steer_map_path path to csv file for steer conversion map + */ + SimModelActuationCmd( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, bool convert_steer_cmd, + std::string accel_map_path, std::string brake_map_path, std::string steer_map_path); + + /** + * @brief default destructor + */ + ~SimModelActuationCmd() = default; + + ActuationMap accel_map_; + ActuationMap brake_map_; + ActuationMap steer_map_; + + bool convert_accel_cmd_; + bool convert_brake_cmd_; + bool convert_steer_cmd_; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U { + ACCEL_DES = 0, + BRAKE_DES, + SLOPE_ACCX, + STEER_DES, + GEAR, + }; + + const double vx_lim_; //!< @brief velocity limit [m/s] + const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const double steer_lim_; //!< @brief steering limit [rad] + const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_; //!< @brief vehicle wheelbase length [m] + + std::deque accel_input_queue_; //!< @brief buffer for accel command + std::deque brake_input_queue_; //!< @brief buffer for brake command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double accel_delay_; //!< @brief time delay for accel command [s] + const double accel_time_constant_; //!< @brief time constant for accel dynamics + const double brake_delay_; //!< @brief time delay for brake command [s] + const double brake_time_constant_; //!< @brief time constant for brake dynamics + const double steer_delay_; //!< @brief time delay for steering command [s] + const double steer_time_constant_; //!< @brief time constant for steering dynamics + const double steer_bias_; //!< @brief steering angle bias [rad] + const std::string path_; //!< @brief conversion map path + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + double getVy() override; + + /** + * @brief get vehicle longitudinal acceleration + */ + double getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time delay steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; + + /** + * @brief update state considering current gear + * @param [in] state current state + * @param [in] prev_state previous state + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) + * @param [in] dt delta time to update state + */ + void updateStateWithGear( + Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, + const double dt); +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index b83a831341ac1..ea0653c879727 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -53,7 +53,7 @@ class AccelerationMap double getAcceleration(const double acc_des, const double vel) const { std::vector interpolated_acc_vec; - const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "acc: vel"); + const double clamped_vel = CSVLoader::clampValue(vel, vel_index_); // (throttle, vel, acc) map => (throttle, acc) map by fixing vel for (const auto & acc_vec : acceleration_map_) { @@ -62,7 +62,7 @@ class AccelerationMap // calculate throttle // When the desired acceleration is smaller than the throttle area, return min acc // When the desired acceleration is greater than the throttle area, return max acc - const double clamped_acc = CSVLoader::clampValue(acc_des, acc_index_, "acceleration: acc"); + const double clamped_acc = CSVLoader::clampValue(acc_des, acc_index_); const double acc = interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); return acc; diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index fd01f42928947..afdbeb120e2d3 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -14,11 +14,13 @@ import launch from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node import launch_ros.parameter_descriptions from launch_ros.substitutions import FindPackageShare +from launch_xml.launch_description_sources import XMLLaunchDescriptionSource import yaml @@ -57,6 +59,7 @@ def launch_setup(context, *args, **kwargs): ("input/vector_map", "/map/vector_map"), ("input/initialpose", "/initialpose3d"), ("input/ackermann_control_command", "/control/command/control_cmd"), + ("input/actuation_command", "/control/command/actuation_cmd"), ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"), ("input/gear_command", "/control/command/gear_cmd"), ("input/manual_gear_command", "/vehicle/command/manual_gear_command"), @@ -77,7 +80,35 @@ def launch_setup(context, *args, **kwargs): ], ) - return [simple_planning_simulator_node] + # Determine if we should launch raw_vehicle_cmd_converter based on the vehicle_model_type + with open(simulator_model_param_path, "r") as f: + simulator_model_param_yaml = yaml.safe_load(f) + launch_vehicle_cmd_converter = ( + simulator_model_param_yaml["/**"]["ros__parameters"].get("vehicle_model_type") + == "ACTUATION_CMD" + ) + + # launch_vehicle_cmd_converter = False # tmp + + # 1) Launch only simple_planning_simulator_node + if not launch_vehicle_cmd_converter: + return [simple_planning_simulator_node] + # 2) Launch raw_vehicle_cmd_converter too + # vehicle_launch_pkg = LaunchConfiguration("vehicle_model").perform(context) + "_launch" + raw_vehicle_converter_node = IncludeLaunchDescription( + XMLLaunchDescriptionSource( + [ + FindPackageShare("autoware_raw_vehicle_cmd_converter"), + "/launch/raw_vehicle_converter.launch.xml", + ] + ), + launch_arguments={ + "config_file": LaunchConfiguration("raw_vehicle_cmd_converter_param_path").perform( + context + ), + }.items(), + ) + return [simple_planning_simulator_node, raw_vehicle_converter_node] def generate_launch_description(): @@ -123,4 +154,24 @@ def add_launch_arg(name: str, default_value=None, description=None): ], ) + # If you use the simulator of the actuation_cmd, you need to start the raw_vehicle_cmd_converter, and the following are optional parameters. + # Please specify the parameter for that. + # The default is the one from autoware_raw_vehicle_cmd_converter, but if you want to use a specific vehicle, please specify the one from {vehicle_model}_launch. + add_launch_arg( + "raw_vehicle_cmd_converter_param_path", + [ + FindPackageShare("autoware_raw_vehicle_cmd_converter"), + "/config/raw_vehicle_cmd_converter.param.yaml", + ], + ) + # NOTE: This is an argument that is not defined in the universe. + # If you use `{vehicle_model}_launch`, you may need to pass `csv_accel_brake_map_path`. + add_launch_arg( + "csv_accel_brake_map_path", + [ + FindPackageShare("autoware_raw_vehicle_cmd_converter"), + "/data/default", + ], + ) + return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 71771feaf78e7..3095e060a71bf 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -115,9 +115,6 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_init_twist_ = create_subscription( "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1)); - sub_ackermann_cmd_ = create_subscription( - "input/ackermann_control_command", QoS{1}, - [this](const Control::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); sub_manual_ackermann_cmd_ = create_subscription( "input/manual_ackermann_control_command", QoS{1}, [this](const Control::ConstSharedPtr msg) { current_manual_ackermann_cmd_ = *msg; }); @@ -144,6 +141,23 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt sub_engage_ = create_subscription( "input/engage", rclcpp::QoS{1}, std::bind(&SimplePlanningSimulator::on_engage, this, _1)); + // Determine input command type based on vehicle_model_type + // NOTE: + // Initial value must be set to current_input_command_ with the correct type. + // If not, the vehicle_model will not be updated, and it will die when publishing the state. + const auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER_VEL"); + if (vehicle_model_type_str == "ACTUATION_CMD") { + current_input_command_ = ActuationCommandStamped(); + sub_actuation_cmd_ = create_subscription( + "input/actuation_command", QoS{1}, + [this](const ActuationCommandStamped::ConstSharedPtr msg) { current_input_command_ = *msg; }); + } else { // default command type is ACKERMANN + current_input_command_ = Control(); + sub_ackermann_cmd_ = create_subscription( + "input/ackermann_control_command", QoS{1}, + [this](const Control::ConstSharedPtr msg) { current_input_command_ = *msg; }); + } + pub_control_mode_report_ = create_publisher("output/control_mode_report", QoS{1}); pub_gear_report_ = create_publisher("output/gear_report", QoS{1}); @@ -175,7 +189,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt rmw_qos_profile_services_default, group_api_service_); // set vehicle model type - initialize_vehicle_model(); + initialize_vehicle_model(vehicle_model_type_str); // set initialize source const auto initialize_source = declare_parameter("initialize_source", "INITIAL_POSE_TOPIC"); @@ -211,12 +225,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt current_manual_gear_cmd_.command = GearCommand::PARK; } -void SimplePlanningSimulator::initialize_vehicle_model() +void SimplePlanningSimulator::initialize_vehicle_model(const std::string & vehicle_model_type_str) { - const auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER_VEL"); - - RCLCPP_DEBUG(this->get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); - const double vel_lim = declare_parameter("vel_lim", 50.0); const double vel_rate_lim = declare_parameter("vel_rate_lim", 7.0); const double steer_lim = declare_parameter("steer_lim", 1.0); @@ -296,7 +306,30 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_ptr_ = std::make_shared( timer_sampling_time_ms_ / 1000.0, model_module_paths, model_param_paths, model_class_names); - + } else if (vehicle_model_type_str == "ACTUATION_CMD") { + vehicle_model_type_ = VehicleModelType::ACTUATION_CMD; + + // time delay + const double accel_time_delay = declare_parameter("accel_time_delay"); + const double accel_time_constant = declare_parameter("accel_time_constant"); + const double brake_time_delay = declare_parameter("brake_time_delay"); + const double brake_time_constant = declare_parameter("brake_time_constant"); + + // command conversion flag + const bool convert_accel_cmd = declare_parameter("convert_accel_cmd"); + const bool convert_brake_cmd = declare_parameter("convert_brake_cmd"); + const bool convert_steer_cmd = declare_parameter("convert_steer_cmd"); + + // actuation conversion map + const std::string accel_map_path = declare_parameter("accel_map_path"); + const std::string brake_map_path = declare_parameter("brake_map_path"); + const std::string steer_map_path = declare_parameter("steer_map_path"); + + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + accel_time_delay, accel_time_constant, brake_time_delay, brake_time_constant, + steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, convert_brake_cmd, + convert_steer_cmd, accel_map_path, brake_map_path, steer_map_path); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } @@ -378,7 +411,7 @@ void SimplePlanningSimulator::on_timer() if (current_control_mode_.mode == ControlModeReport::AUTONOMOUS) { vehicle_model_ptr_->setGear(current_gear_cmd_.command); - set_input(current_ackermann_cmd_, acc_by_slope); + set_input(current_input_command_, acc_by_slope); } else { vehicle_model_ptr_->setGear(current_manual_gear_cmd_.command); set_input(current_manual_ackermann_cmd_, acc_by_slope); @@ -473,6 +506,37 @@ void SimplePlanningSimulator::on_set_pose( response->status = tier4_api_utils::response_success(); } +void SimplePlanningSimulator::set_input(const InputCommand & cmd, const double acc_by_slope) +{ + std::visit( + [this, acc_by_slope](auto && arg) { + using T = std::decay_t; + if constexpr (std::is_same_v) { + set_input(arg, acc_by_slope); + } else if constexpr (std::is_same_v) { + set_input(arg, acc_by_slope); + } else { + throw std::invalid_argument("Invalid input command type"); + } + }, + cmd); +} + +void SimplePlanningSimulator::set_input( + const ActuationCommandStamped & cmd, const double acc_by_slope) +{ + const auto accel = cmd.actuation.accel_cmd; + const auto brake = cmd.actuation.brake_cmd; + const auto steer = cmd.actuation.steer_cmd; + const auto gear = vehicle_model_ptr_->getGear(); + + Eigen::VectorXd input(vehicle_model_ptr_->getDimU()); + input << accel, brake, acc_by_slope, steer, gear; + + // VehicleModelType::ACTUATION_COMMAND + vehicle_model_ptr_->setInput(input); +} + void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by_slope) { const auto steer = cmd.lateral.steering_tire_angle; @@ -612,7 +676,8 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED_WO_FALL_GUARD || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::ACTUATION_CMD) { state << x, y, yaw, vx, steer, accx; } vehicle_model_ptr_->setState(state); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp index c9eb7a5a237fb..1189477a4f45b 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp @@ -26,6 +26,8 @@ CSVLoader::CSVLoader(const std::string & csv_path) bool CSVLoader::readCSV(Table & result, const char delim) { + // NOTE: Table[i][j] represents the element in the i-th row and j-th column + std::ifstream ifs(csv_path_); if (!ifs.is_open()) { std::cerr << "Cannot open " << csv_path_.c_str() << std::endl; @@ -119,6 +121,8 @@ Map CSVLoader::getMap(const Table & table) std::vector CSVLoader::getRowIndex(const Table & table) { + // NOTE: table[0][i] represents the element in the 0-th row and i-th column + // This means that we are getting the index of each column in the 0-th row std::vector index = {}; for (size_t i = 1; i < table[0].size(); i++) { index.push_back(std::stod(table[0][i])); @@ -128,6 +132,8 @@ std::vector CSVLoader::getRowIndex(const Table & table) std::vector CSVLoader::getColumnIndex(const Table & table) { + // NOTE: table[i][0] represents the element in the i-th row and 0-th column + // This means that we are getting the index of each row in the 0-th column std::vector index = {}; for (size_t i = 1; i < table.size(); i++) { index.push_back(std::stod(table[i][0])); @@ -135,15 +141,11 @@ std::vector CSVLoader::getColumnIndex(const Table & table) return index; } -double CSVLoader::clampValue( - const double val, const std::vector & ranges, const std::string & name) +double CSVLoader::clampValue(const double val, const std::vector & ranges) { const double max_value = *std::max_element(ranges.begin(), ranges.end()); const double min_value = *std::min_element(ranges.begin(), ranges.end()); if (val < min_value || max_value < val) { - std::cerr << "Input " << name << ": " << val - << " is out of range. use closest value. Please update the conversion map" - << std::endl; return std::min(std::max(val, min_value), max_value); } return val; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp new file mode 100644 index 0000000000000..16eb6e86d778c --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -0,0 +1,259 @@ +// 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 "simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" + +#include "autoware_vehicle_msgs/msg/gear_command.hpp" + +#include + +bool ActuationMap::readActuationMapFromCSV(const std::string & csv_path, const bool validation) +{ + CSVLoader csv(csv_path); + std::vector> table; + + if (!csv.readCSV(table)) { + return false; + } + + state_index_ = CSVLoader::getRowIndex(table); + actuation_index_ = CSVLoader::getColumnIndex(table); + actuation_map_ = CSVLoader::getMap(table); + if (validation && !CSVLoader::validateMap(actuation_map_, true)) { + return false; + } + return true; +} + +double ActuationMap::getControlCommand(const double actuation, const double state) const +{ + std::vector interpolated_control_vec{}; + const double clamped_state = CSVLoader::clampValue(state, state_index_); + + for (std::vector control_command_values : actuation_map_) { + interpolated_control_vec.push_back( + interpolation::lerp(state_index_, control_command_values, clamped_state)); + } + + const double clamped_actuation = CSVLoader::clampValue(actuation, actuation_index_); + return interpolation::lerp(actuation_index_, interpolated_control_vec, clamped_actuation); +} + +SimModelActuationCmd::SimModelActuationCmd( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, bool convert_steer_cmd, + std::string accel_map_path, std::string brake_map_path, std::string steer_map_path) +: SimModelInterface(6 /* dim x */, 5 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), + accel_delay_(accel_delay), + accel_time_constant_(std::max(accel_time_constant, MIN_TIME_CONSTANT)), + brake_delay_(brake_delay), + brake_time_constant_(std::max(brake_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_bias_(steer_bias) +{ + initializeInputQueue(dt); + convert_accel_cmd_ = convert_accel_cmd && accel_map_.readActuationMapFromCSV(accel_map_path); + convert_brake_cmd_ = convert_brake_cmd && brake_map_.readActuationMapFromCSV(brake_map_path); + convert_steer_cmd_ = convert_steer_cmd && steer_map_.readActuationMapFromCSV(steer_map_path); +} + +double SimModelActuationCmd::getX() +{ + return state_(IDX::X); +} +double SimModelActuationCmd::getY() +{ + return state_(IDX::Y); +} +double SimModelActuationCmd::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelActuationCmd::getVx() +{ + return state_(IDX::VX); +} +double SimModelActuationCmd::getVy() +{ + return 0.0; +} +double SimModelActuationCmd::getAx() +{ + return state_(IDX::ACCX); +} +double SimModelActuationCmd::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +double SimModelActuationCmd::getSteer() +{ + return state_(IDX::STEER) + steer_bias_; +} +void SimModelActuationCmd::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + accel_input_queue_.push_back(input_(IDX_U::ACCEL_DES)); + delayed_input(IDX_U::ACCEL_DES) = accel_input_queue_.front(); + accel_input_queue_.pop_front(); + + brake_input_queue_.push_back(input_(IDX_U::BRAKE_DES)); + delayed_input(IDX_U::BRAKE_DES) = brake_input_queue_.front(); + brake_input_queue_.pop_front(); + + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); + + delayed_input(IDX_U::GEAR) = input_(IDX_U::GEAR); + delayed_input(IDX_U::SLOPE_ACCX) = input_(IDX_U::SLOPE_ACCX); + + const auto prev_state = state_; + updateRungeKutta(dt, delayed_input); + + // take velocity limit explicitly + state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); + + // consider gear + // update position and velocity first, and then acceleration is calculated naturally + updateStateWithGear(state_, prev_state, gear_, dt); +} + +void SimModelActuationCmd::initializeInputQueue(const double & dt) +{ + const size_t accel_input_queue_size = static_cast(round(accel_delay_ / dt)); + accel_input_queue_.resize(accel_input_queue_size); + std::fill(accel_input_queue_.begin(), accel_input_queue_.end(), 0.0); + + const size_t brake_input_queue_size = static_cast(round(brake_delay_ / dt)); + brake_input_queue_.resize(brake_input_queue_size); + std::fill(brake_input_queue_.begin(), brake_input_queue_.end(), 0.0); + + const size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + steer_input_queue_.resize(steer_input_queue_size); + std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); +} + +Eigen::VectorXd SimModelActuationCmd::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + using autoware_vehicle_msgs::msg::GearCommand; + + const double vel = std::clamp(state(IDX::VX), -vx_lim_, vx_lim_); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + + const double accel = input(IDX_U::ACCEL_DES); + const double brake = input(IDX_U::BRAKE_DES); + const auto gear = input(IDX_U::GEAR); + + // 1) calculate acceleration by accel and brake command + const double acc_des = std::clamp( + std::invoke([&]() -> double { + // Select the non-zero value between accel and brake and calculate the acceleration + if (convert_accel_cmd_ && accel > 0.0) { + // convert accel command to acceleration + return accel_map_.getControlCommand(accel, vel); + } else if (convert_brake_cmd_ && brake > 0.0) { + // convert brake command to acceleration + return brake_map_.getControlCommand(brake, vel); + } else { + // if conversion is disabled, accel command is directly used as acceleration + return accel; + } + }), + -vx_rate_lim_, vx_rate_lim_); + // add slope acceleration considering the gear state + const double acc_by_slope = input(IDX_U::SLOPE_ACCX); + const double acc = std::invoke([&]() -> double { + if (gear == GearCommand::NONE || gear == GearCommand::PARK) { + return 0.0; + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { + return -acc_des + acc_by_slope; + } + return acc_des + acc_by_slope; + }); + const double acc_time_constant = accel > 0.0 ? accel_time_constant_ : brake_time_constant_; + + // 2) calculate steering rate by steer command + const double steer_rate = std::clamp( + std::invoke([&]() -> double { + // if conversion is enabled, calculate steering rate from steer command + if (convert_steer_cmd_) { + // convert steer command to steer rate + return steer_map_.getControlCommand(input(IDX_U::STEER_DES), steer) / steer_time_constant_; + } + // otherwise, steer command is desired steering angle, so calculate steering rate from the + // difference between the desired steering angle and the current steering angle. + const double steer_des = std::clamp(input(IDX_U::STEER_DES), -steer_lim_, steer_lim_); + return -(getSteer() - steer_des) / steer_time_constant_; + }), + -steer_rate_lim_, steer_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = acc; + d_state(IDX::STEER) = steer_rate; + d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant; + + return d_state; +} + +void SimModelActuationCmd::updateStateWithGear( + Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) +{ + using autoware_vehicle_msgs::msg::GearCommand; + if ( + gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || + gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || + gear == GearCommand::DRIVE_7 || gear == GearCommand::DRIVE_8 || gear == GearCommand::DRIVE_9 || + gear == GearCommand::DRIVE_10 || gear == GearCommand::DRIVE_11 || + gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || + gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || + gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { + if (state(IDX::VX) < 0.0) { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + } + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { + if (state(IDX::VX) > 0.0) { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + } + } else { // including 'gear == GearCommand::PARK' + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + } +} diff --git a/simulator/simple_planning_simulator/test/actuation_cmd_map/accel_map.csv b/simulator/simple_planning_simulator/test/actuation_cmd_map/accel_map.csv new file mode 100644 index 0000000000000..8b83ecf68d628 --- /dev/null +++ b/simulator/simple_planning_simulator/test/actuation_cmd_map/accel_map.csv @@ -0,0 +1,6 @@ +default,0.0,13.89 +0, 3.0, 2.9 +0.3, 4.0, 3.9 +0.5, 5.0, 4.9 +0.7, 8.0, 7.9 +1.0, 10.0, 9.9 diff --git a/simulator/simple_planning_simulator/test/actuation_cmd_map/brake_map.csv b/simulator/simple_planning_simulator/test/actuation_cmd_map/brake_map.csv new file mode 100644 index 0000000000000..adf2c80950bb1 --- /dev/null +++ b/simulator/simple_planning_simulator/test/actuation_cmd_map/brake_map.csv @@ -0,0 +1,10 @@ +default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 +0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 +0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51 +0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96 +0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633 +0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 +0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 +0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 +0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 +0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955 diff --git a/simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv b/simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv new file mode 100644 index 0000000000000..7adb0f6a9e583 --- /dev/null +++ b/simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv @@ -0,0 +1,4 @@ +default,-0.6,0,0.6 +-10,-0.03,-0.03,-0.03 +0,0.0,0.0,0.0 +10,0.03,0.03,0.03 diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index c61237f83e5bd..73762150f8e1d 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "ament_index_cpp/get_package_share_directory.hpp" #include "gtest/gtest.h" #include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "tf2/utils.h" +#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" + #ifdef ROS_DISTRO_GALACTIC #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #else @@ -28,7 +31,9 @@ using autoware_control_msgs::msg::Control; using autoware_vehicle_msgs::msg::GearCommand; using geometry_msgs::msg::PoseWithCovarianceStamped; using nav_msgs::msg::Odometry; +using tier4_vehicle_msgs::msg::ActuationCommandStamped; +using simulation::simple_planning_simulator::InputCommand; using simulation::simple_planning_simulator::SimplePlanningSimulator; std::string toStrInfo(const Odometry & o) @@ -42,6 +47,23 @@ std::string toStrInfo(const Odometry & o) return ss.str(); } +enum class CommandType { Ackermann, Actuation }; + +struct Ackermann +{ + double steer = 0.0; + double steer_rate = 0.0; + double vel = 0.0; + double acc = 0.0; + double jerk = 0.0; +}; +struct Actuation +{ + double steer = 0.0; + double accel = 0.0; + double brake = 0.0; +}; + class PubSubNode : public rclcpp::Node { public: @@ -52,6 +74,8 @@ class PubSubNode : public rclcpp::Node [this](const Odometry::ConstSharedPtr msg) { current_odom_ = msg; }); pub_ackermann_command_ = create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); + pub_actuation_command_ = + create_publisher("input/actuation_command", rclcpp::QoS{1}); pub_initialpose_ = create_publisher("input/initialpose", rclcpp::QoS{1}); pub_gear_cmd_ = create_publisher("input/gear_command", rclcpp::QoS{1}); @@ -59,6 +83,7 @@ class PubSubNode : public rclcpp::Node rclcpp::Subscription::SharedPtr current_odom_sub_; rclcpp::Publisher::SharedPtr pub_ackermann_command_; + rclcpp::Publisher::SharedPtr pub_actuation_command_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; rclcpp::Publisher::SharedPtr pub_initialpose_; @@ -74,19 +99,35 @@ class PubSubNode : public rclcpp::Node * @param [in] acc [m/s²] acceleration * @param [in] jerk [m/s3] jerk */ -Control cmdGen( - const builtin_interfaces::msg::Time & t, double steer, double steer_rate, double vel, double acc, - double jerk) +Control ackermannCmdGen(const builtin_interfaces::msg::Time & t, const Ackermann & ackermann_cmd) { Control cmd; cmd.stamp = t; cmd.lateral.stamp = t; - cmd.lateral.steering_tire_angle = steer; - cmd.lateral.steering_tire_rotation_rate = steer_rate; + cmd.lateral.steering_tire_angle = ackermann_cmd.steer; + cmd.lateral.steering_tire_rotation_rate = ackermann_cmd.steer_rate; cmd.longitudinal.stamp = t; - cmd.longitudinal.velocity = vel; - cmd.longitudinal.acceleration = acc; - cmd.longitudinal.jerk = jerk; + cmd.longitudinal.velocity = ackermann_cmd.vel; + cmd.longitudinal.acceleration = ackermann_cmd.acc; + cmd.longitudinal.jerk = ackermann_cmd.jerk; + return cmd; +} + +/** + * @brief Generate an ActuationCommandStamped message + * @param [in] t timestamp + * @param [in] accel_cmd accel actuation command + * @param [in] brake_cmd brake actuation command + * @param [in] steer_cmd steer actuation command + */ +ActuationCommandStamped actuationCmdGen( + const builtin_interfaces::msg::Time & t, const Actuation & actuation_cmd) +{ + ActuationCommandStamped cmd; + cmd.header.stamp = t; + cmd.actuation.accel_cmd = actuation_cmd.accel; + cmd.actuation.brake_cmd = actuation_cmd.brake; + cmd.actuation.steer_cmd = actuation_cmd.steer; return cmd; } @@ -120,14 +161,17 @@ void sendGear( /** * @brief publish the given command message - * @param [in] cmd command to publish + * @param [in] cmd_orig command to publish * @param [in] sim_node pointer to the simulation node * @param [in] pub_sub_node pointer to the node used for communication */ -void sendCommand( - const Control & cmd, rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) +void sendAckermannCommand( + const Control & cmd_orig, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) { + auto cmd = cmd_orig; for (int i = 0; i < 150; ++i) { + cmd.stamp = sim_node->now(); pub_sub_node->pub_ackermann_command_->publish(cmd); rclcpp::spin_some(sim_node); rclcpp::spin_some(pub_sub_node); @@ -135,6 +179,34 @@ void sendCommand( } } +void sendActuationCommand( + const ActuationCommandStamped & cmd_orig, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) +{ + auto cmd = cmd_orig; + for (int i = 0; i < 150; ++i) { + cmd.header.stamp = sim_node->now(); + pub_sub_node->pub_actuation_command_->publish(cmd); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +void sendCommand( + const CommandType & cmd_type, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node, const builtin_interfaces::msg::Time & t, + const Ackermann & ackermann_cmd, const Actuation & actuation_cmd) +{ + if (cmd_type == CommandType::Ackermann) { + sendAckermannCommand(ackermannCmdGen(t, ackermann_cmd), sim_node, pub_sub_node); + } else if (cmd_type == CommandType::Actuation) { + sendActuationCommand(actuationCmdGen(t, actuation_cmd), sim_node, pub_sub_node); + } else { + throw std::invalid_argument("command type is unexpected."); + } +} + // Check which direction the vehicle is heading on the baselink coordinates. // y // | @@ -195,7 +267,8 @@ void declareVehicleInfoParams(rclcpp::NodeOptions & node_options) // Send a control command and run the simulation. // Then check if the vehicle is moving in the desired direction. -class TestSimplePlanningSimulator : public ::testing::TestWithParam +class TestSimplePlanningSimulator +: public ::testing::TestWithParam> { }; @@ -203,7 +276,9 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) { rclcpp::init(0, nullptr); - const auto vehicle_model_type = GetParam(); + const auto params = GetParam(); + const auto command_type = std::get<0>(params); + const auto vehicle_model_type = std::get<1>(params); std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; rclcpp::NodeOptions node_options; @@ -211,6 +286,21 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) node_options.append_parameter_override("vehicle_model_type", vehicle_model_type); node_options.append_parameter_override("initial_engage_state", true); node_options.append_parameter_override("add_measurement_noise", false); + node_options.append_parameter_override("accel_time_delay", 0.2); + node_options.append_parameter_override("accel_time_constant", 0.2); + node_options.append_parameter_override("brake_time_delay", 0.2); + node_options.append_parameter_override("brake_time_constant", 0.2); + node_options.append_parameter_override("convert_accel_cmd", true); + node_options.append_parameter_override("convert_brake_cmd", true); + node_options.append_parameter_override("convert_steer_cmd", true); + const auto share_dir = ament_index_cpp::get_package_share_directory("simple_planning_simulator"); + const auto accel_map_path = share_dir + "/test/actuation_cmd_map/accel_map.csv"; + const auto brake_map_path = share_dir + "/test/actuation_cmd_map/brake_map.csv"; + const auto steer_map_path = share_dir + "/test/actuation_cmd_map/steer_map.csv"; + node_options.append_parameter_override("accel_map_path", accel_map_path); + node_options.append_parameter_override("brake_map_path", brake_map_path); + node_options.append_parameter_override("steer_map_path", steer_map_path); + declareVehicleInfoParams(node_options); const auto sim_node = std::make_shared(node_options); @@ -220,15 +310,31 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) const double target_acc = 5.0f; const double target_steer = 0.2f; + // NOTE: As the value of the actuation map is known, roughly determine whether it is + // acceleration or braking, and whether it turns left or right, and generate an actuation + // command. So do not change the map. If it is necessary, you need to change this parameters as + // well. + const double target_steer_actuation = 10.0f; + const double target_accel_actuation = 0.5f; + // const double target_brake_actuation = 0.5f; // unused for now. + auto _resetInitialpose = [&]() { resetInitialpose(sim_node, pub_sub_node); }; auto _sendFwdGear = [&]() { sendGear(GearCommand::DRIVE, sim_node, pub_sub_node); }; auto _sendBwdGear = [&]() { sendGear(GearCommand::REVERSE, sim_node, pub_sub_node); }; - auto _sendCommand = [&](const auto & _cmd) { sendCommand(_cmd, sim_node, pub_sub_node); }; + auto _sendCommand = [&](auto ackermann_cmd, auto actuation_cmd) { + const auto t = sim_node->now(); + sendCommand(command_type, sim_node, pub_sub_node, t, ackermann_cmd, actuation_cmd); + }; // check pub-sub connections { size_t expected = 1; - EXPECT_EQ(pub_sub_node->pub_ackermann_command_->get_subscription_count(), expected); + // actuation or ackermann must be subscribed + const auto sub_command_count = + (command_type == CommandType::Actuation) + ? pub_sub_node->pub_actuation_command_->get_subscription_count() + : pub_sub_node->pub_ackermann_command_->get_subscription_count(); + EXPECT_EQ(sub_command_count, expected); EXPECT_EQ(pub_sub_node->pub_gear_cmd_->get_subscription_count(), expected); EXPECT_EQ(pub_sub_node->pub_initialpose_->get_subscription_count(), expected); EXPECT_EQ(pub_sub_node->current_odom_sub_->get_publisher_count(), expected); @@ -241,39 +347,50 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) // go forward _resetInitialpose(); _sendFwdGear(); - _sendCommand(cmdGen(sim_node->now(), 0.0f, 0.0f, target_vel, target_acc, 0.0f)); + _sendCommand( + Ackermann{0.0f, 0.0f, target_vel, target_acc, 0.0f}, + Actuation{0.0f, target_accel_actuation, 0.0f}); isOnForward(*(pub_sub_node->current_odom_), init_state); // go backward // NOTE: positive acceleration with reverse gear drives the vehicle backward. _resetInitialpose(); _sendBwdGear(); - _sendCommand(cmdGen(sim_node->now(), 0.0f, 0.0f, -target_vel, target_acc, 0.0f)); + _sendCommand( + Ackermann{0.0f, 0.0f, -target_vel, target_acc, 0.0f}, + Actuation{0.0f, target_accel_actuation, 0.0f}); isOnBackward(*(pub_sub_node->current_odom_), init_state); // go forward left _resetInitialpose(); _sendFwdGear(); - _sendCommand(cmdGen(sim_node->now(), target_steer, 0.0f, target_vel, target_acc, 0.0f)); + _sendCommand( + Ackermann{target_steer, 0.0f, target_vel, target_acc, 0.0f}, + Actuation{target_steer_actuation, target_accel_actuation, 0.0f}); isOnForwardLeft(*(pub_sub_node->current_odom_), init_state); // go backward right // NOTE: positive acceleration with reverse gear drives the vehicle backward. _resetInitialpose(); _sendBwdGear(); - _sendCommand(cmdGen(sim_node->now(), -target_steer, 0.0f, -target_vel, target_acc, 0.0f)); + _sendCommand( + Ackermann{-target_steer, 0.0f, -target_vel, target_acc, 0.0f}, + Actuation{-target_steer_actuation, target_accel_actuation, 0.0f}); isOnBackwardRight(*(pub_sub_node->current_odom_), init_state); rclcpp::shutdown(); } -// clang-format off -const std::string VEHICLE_MODEL_LIST[] = { // NOLINT - "IDEAL_STEER_VEL", "IDEAL_STEER_ACC", "IDEAL_STEER_ACC_GEARED", - "DELAY_STEER_VEL", "DELAY_STEER_ACC", "DELAY_STEER_ACC_GEARED", - "DELAY_STEER_ACC_GEARED_WO_FALL_GUARD" -}; -// clang-format on - INSTANTIATE_TEST_SUITE_P( - TestForEachVehicleModel, TestSimplePlanningSimulator, ::testing::ValuesIn(VEHICLE_MODEL_LIST)); + TestForEachVehicleModelTrue, TestSimplePlanningSimulator, + ::testing::Values( + /* Ackermann type */ + std::make_tuple(CommandType::Ackermann, "IDEAL_STEER_VEL"), + std::make_tuple(CommandType::Ackermann, "IDEAL_STEER_ACC"), + std::make_tuple(CommandType::Ackermann, "IDEAL_STEER_ACC_GEARED"), + std::make_tuple(CommandType::Ackermann, "DELAY_STEER_VEL"), + std::make_tuple(CommandType::Ackermann, "DELAY_STEER_ACC"), + std::make_tuple(CommandType::Ackermann, "DELAY_STEER_ACC_GEARED"), + std::make_tuple(CommandType::Ackermann, "DELAY_STEER_ACC_GEARED_WO_FALL_GUARD"), + /* Actuation type */ + std::make_tuple(CommandType::Actuation, "ACTUATION_CMD"))); 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/emergency_handler/package.xml b/system/emergency_handler/package.xml index 099e8a77d191e..e16c1b60a5be0 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -20,8 +20,6 @@ nav_msgs rclcpp rclcpp_components - std_msgs - std_srvs tier4_system_msgs ament_lint_auto 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/package.xml b/system/mrm_handler/package.xml index a2c3397296db3..6e08b261f09c0 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -13,15 +13,11 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_control_msgs - autoware_system_msgs autoware_universe_utils autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components - std_msgs - std_srvs tier4_system_msgs ament_lint_auto 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/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index 85a88d2430bce..50ac4363da609 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -684,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()); 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